diff --git a/assets/toppra b/assets/toppra
new file mode 160000
index 00000000..3089c789
--- /dev/null
+++ b/assets/toppra
@@ -0,0 +1 @@
+Subproject commit 3089c7897a5711aceb39d25919aca8c57b5c5948
diff --git a/cmvr-es/common/config/cabin_robot.xml b/cmvr-es/common/config/cabin_robot.xml
index 574f5204..18a107c9 100644
--- a/cmvr-es/common/config/cabin_robot.xml
+++ b/cmvr-es/common/config/cabin_robot.xml
@@ -32,44 +32,44 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
diff --git a/cmvr-es/common/config/camera_config/camera_config.pb.txt b/cmvr-es/common/config/camera_config/camera_config.pb.txt
index db18cd7e..6d0e71da 100644
--- a/cmvr-es/common/config/camera_config/camera_config.pb.txt
+++ b/cmvr-es/common/config/camera_config/camera_config.pb.txt
@@ -1,6 +1,6 @@
realsense_cameras {
id: "cam2"
- serialNumber: "123456789012"
+ serialNumber: "243122072252"
width: 1280
height: 720
fps: 30
@@ -10,7 +10,7 @@ realsense_cameras {
align_mode: ALIGN_MODE_COLOR
buffer_size: 30
sync: true
- enable: false
+ enable: true
}
uvc_cameras {
@@ -23,5 +23,5 @@ uvc_cameras {
camera_mode: CAMERA_MODE_PHOTO
stream_mode: STREAM_MODE_RGB
buffer_size: 30
- enable: true
+ enable: false
}
diff --git a/cmvr-es/common/utils/config_helper/src/config_setting.cpp b/cmvr-es/common/utils/config_helper/src/config_setting.cpp
index 0d3e514e..2e32d6ed 100644
--- a/cmvr-es/common/utils/config_helper/src/config_setting.cpp
+++ b/cmvr-es/common/utils/config_helper/src/config_setting.cpp
@@ -2,13 +2,17 @@
#include
+// static std::string basePath()
+// {
+// char* cwd = ::getcwd(nullptr, 0);
+// if (!cwd) return {};
+// std::string s(cwd);
+// std::free(cwd);
+// return s + "/config/";
+// }
static std::string basePath()
{
- char* cwd = ::getcwd(nullptr, 0);
- if (!cwd) return {};
- std::string s(cwd);
- std::free(cwd);
- return s + "/config/";
+ return "/home/lgv/cmvr/cmvr-es/output/bin/config/";
}
DEFINE_string(pinocchio_qp_ik_solver_config_file,
diff --git a/cmvr-es/controller/CMakeLists.txt b/cmvr-es/controller/CMakeLists.txt
index f8d327cb..01685ec0 100644
--- a/cmvr-es/controller/CMakeLists.txt
+++ b/cmvr-es/controller/CMakeLists.txt
@@ -1,5 +1,5 @@
-find_package(VISP REQUIRED)
+#find_package(VISP REQUIRED)
# 如果报 relocation ... can not be used when making a shared object; recompile with -fPIC ，说明SRC 中包含了test文件 ，test
@@ -32,7 +32,24 @@ target_link_libraries(controller PUBLIC
ccd
fcl
cmvr_es::device_manager
- ${VISP_LIBRARIES}
+ visp_vs
+ visp_visual_features
+ visp_vision
+ visp_tt_mi
+ visp_tt
+ visp_me
+ visp_mbt
+ visp_klt
+ visp_dnn_tracker
+ visp_blob
+ visp_sensor
+ visp_robot
+ visp_io
+ visp_imgproc
+ visp_gui
+ visp_detection
+ visp_core
+ visp_ar
pinocchio_default
pinocchio_parsers
)
diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp
index 89827d41..7d1186d5 100644
--- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp
+++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp
@@ -108,7 +108,7 @@ TEST(HumanoidRobotTest,MyRobotTest) {
}
TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
- const XmlNode config("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml");
+ const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
@@ -130,18 +130,18 @@ TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
ASSERT_TRUE(refreshRightArmQ()) << "Failed to read right-arm joint q";
- // std::vector cmd{};
- // cmd = {
- // {"R_SHOULDER_P", 0.0},
- // {"R_SHOULDER_R", 0.0},
- // {"R_SHOULDER_Y", 0.0},
- // {"R_ELBOW_R", 0.0},
- // {"R_WRIST_P", 0.0},
- // {"R_WRIST_Y", 0.0},
- // {"R_WRIST_R", 0.0},
- // };
- auto joint_cmd = buildRightArmJointCmd(q_now, 0.8);
- ASSERT_NO_THROW(robot->servoJ(joint_cmd, 0.8, 0.01));
+ std::vector cmd{};
+ cmd = {
+ // {"R_SHOULDER_P", 0.0},
+ // {"R_SHOULDER_R", 0.0},
+ // {"R_SHOULDER_Y", 0.0},
+ // {"R_ELBOW_R", 0.0},
+ // {"R_WRIST_P", 0.0},
+ // {"R_WRIST_Y", 0.0},
+ {"R_WRIST_R", 0.2},
+ };
+ // auto joint_cmd = buildRightArmJointCmd(q_now, 0.8);
+ ASSERT_NO_THROW(robot->servoJ(cmd, 0.8, 0.01));
while (true) {
std::this_thread::sleep_for(std::chrono::milliseconds(10000));
}
@@ -149,13 +149,13 @@ TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
}
TEST(HumanoidRobotTest,IBVSWithRealRobot) {
- const XmlNode config("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml");
+ const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice("hc01");
- auto camera = dmgr.getDevice("cam4");
+ auto camera = dmgr.getDevice("cam2");
ASSERT_NE(robot, nullptr);
ASSERT_NE(camera, nullptr);
@@ -189,7 +189,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
ibvs_controller.setDepthZGain(1.0);
ASSERT_TRUE(ibvs_controller.init(camera,
- "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
+ "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
"PELVIS_S",
"R_WRIST_R_S",
"R_CAM"))
@@ -213,8 +213,8 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
<< "Failed to extract right-arm 7 joints from robot state";
ibvs_controller.reset(q_now);
- const int max_steps = 300;
- const int log_every = 10;
+ const int max_steps = 30000;
+ const int log_every = 1;
const int cycle_ms = 33;
int ok_steps = 0;
@@ -252,7 +252,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
}
auto joint_cmd = buildRightArmJointCmd(q_cmd_next, 0.8);
- robot->servoJ(joint_cmd, 0.8, dt);
+ // robot->servoJ(joint_cmd, 0.8, dt);
++ok_steps;
if ((step % log_every) == 0) {
diff --git a/cmvr-es/perception/CMakeLists.txt b/cmvr-es/perception/CMakeLists.txt
index da9d6e42..322ca2d1 100644
--- a/cmvr-es/perception/CMakeLists.txt
+++ b/cmvr-es/perception/CMakeLists.txt
@@ -1,4 +1,4 @@
-find_package(VISP REQUIRED)
+#find_package(VISP REQUIRED)
find_package(OpenCV REQUIRED)
add_library(perception SHARED
@@ -7,24 +7,41 @@ add_library(perception SHARED
target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(perception PUBLIC
- ${VISP_LIBRARIES}
+ visp_vs
+ visp_visual_features
+ visp_vision
+ visp_tt_mi
+ visp_tt
+ visp_me
+ visp_mbt
+ visp_klt
+ visp_dnn_tracker
+ visp_blob
+ visp_sensor
+ visp_robot
+ visp_io
+ visp_imgproc
+ visp_gui
+ visp_detection
+ visp_core
+ visp_ar
${OpenCV_LIBS}
)
add_library(cmvr_es::perception ALIAS perception)
install(TARGETS perception LIBRARY DESTINATION lib)
-add_executable(tag_relative_target_3d_test
- src/tag_relative_target_3d_test.cpp
-)
-
-target_link_libraries(tag_relative_target_3d_test
- PRIVATE
- cmvr_es::perception
- cmvr_es::device::realsense_camera
- cmvr_es::proto
- glog
- gtest
- gtest_main
- pthread
-)
+#add_executable(tag_relative_target_3d_test
+# src/tag_relative_target_3d_test.cpp
+#)
+#
+#target_link_libraries(tag_relative_target_3d_test
+# PRIVATE
+# cmvr_es::perception
+# cmvr_es::device::realsense_camera
+# cmvr_es::proto
+# glog
+# gtest
+# gtest_main
+# pthread
+#)