diff --git a/CMakeLists.txt.user b/CMakeLists.txt.user new file mode 100644 index 00000000..a8c05072 --- /dev/null +++ b/CMakeLists.txt.user @@ -0,0 +1,507 @@ + + + + + + EnvironmentId + {e7879c5d-5c93-4462-a263-cf18c9bfc1e7} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + 0 + UTF-8 + false + 4 + false + 80 + true + true + 1 + true + false + 0 + true + true + 0 + 8 + true + 1 + true + true + true + false + + + + ProjectExplorer.Project.PluginSettings + + + true + + + + ProjectExplorer.Project.Target.0 + + Imported Kit + Imported Kit + {7278e3ef-0254-4507-bab5-da24721042c3} + 5 + 0 + 2 + + + /Users/kdarvish/Desktop/icub_ws/robotology-superbuild/robotology/build-walking-teleoperation-Imported_Kit_temporary-Default + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + clean + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Default + Default + CMakeProjectManager.CMakeBuildConfiguration + + + + CMAKE_BUILD_TYPE:STRING=Debug + + /Users/kdarvish/Desktop/icub_ws/robotology-superbuild/robotology/build-walking-teleoperation-Imported_Kit_temporary-Debug + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + clean + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Debug + Debug + CMakeProjectManager.CMakeBuildConfiguration + + + + CMAKE_BUILD_TYPE:STRING=Release + + /Users/kdarvish/Desktop/icub_ws/robotology-superbuild/robotology/build-walking-teleoperation-Imported_Kit_temporary-Release + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + clean + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Release + Release + CMakeProjectManager.CMakeBuildConfiguration + + + + CMAKE_BUILD_TYPE:STRING=RelWithDebInfo + + /Users/kdarvish/Desktop/icub_ws/robotology-superbuild/robotology/build-walking-teleoperation-Imported_Kit_temporary-Release with Debug Information + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + clean + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Release with Debug Information + Release with Debug Information + CMakeProjectManager.CMakeBuildConfiguration + + + + CMAKE_BUILD_TYPE:STRING=MinSizeRel + + /Users/kdarvish/Desktop/icub_ws/robotology-superbuild/robotology/build-walking-teleoperation-Imported_Kit_temporary-Minimum Size Release + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + clean + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Minimum Size Release + Minimum Size Release + CMakeProjectManager.CMakeBuildConfiguration + + + + CMAKE_BUILD_TYPE:STRING=Release + CMAKE_CXX_COMPILER:FILEPATH=/Applications/Xcode.app/Contents/Developer/Toolchains/XcodeDefault.xctoolchain/usr/bin/c++ + CMAKE_C_COMPILER:FILEPATH=/Applications/Xcode.app/Contents/Developer/Toolchains/XcodeDefault.xctoolchain/usr/bin/cc + CMAKE_PREFIX_PATH:PATH=/Users/kdarvish/Desktop/icub_ws/robotology-superbuild/build/install + + /Users/kdarvish/Desktop/icub_ws/robotology-superbuild/build/robotology/walking-teleoperation + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + clean + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Release + Release2 + CMakeProjectManager.CMakeBuildConfiguration + + 6 + + + 0 + Deploy + + ProjectExplorer.BuildSteps.Deploy + + 1 + Deploy Configuration + + ProjectExplorer.DefaultDeployConfiguration + + 1 + + + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + 2 + + OculusRetargetingModule + + CMakeProjectManager.CMakeRunConfiguration.OculusRetargetingModule +/Users/kdarvish/Desktop/icub_ws/robotology-superbuild/robotology/walking-teleoperation/modules/Oculus_module/ + + 3768 + false + true + false + false + true + + /Users/kdarvish/Desktop/icub_ws/robotology-superbuild/build/robotology/walking-teleoperation/modules/Oculus_module + + + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + 2 + + WholeBodyRetargetingModule + + CMakeProjectManager.CMakeRunConfiguration.WholeBodyRetargetingModule +/Users/kdarvish/Desktop/icub_ws/robotology-superbuild/robotology/walking-teleoperation/modules/WBRetargeting_module/ + + 3768 + false + true + false + false + true + + + + + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + 2 + + XsensRetargetingModule + + CMakeProjectManager.CMakeRunConfiguration.XsensRetargetingModule +/Users/kdarvish/Desktop/icub_ws/robotology-superbuild/robotology/walking-teleoperation/modules/Xsens_module/ + + 3768 + false + true + false + false + true + + /Users/kdarvish/Desktop/icub_ws/robotology-superbuild/build/robotology/walking-teleoperation/modules/Xsens_module + + 3 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 20 + + + Version + 20 + + diff --git a/README.md b/README.md index ca989f63..10117dd1 100644 --- a/README.md +++ b/README.md @@ -7,6 +7,8 @@ The suite includes: * **Virtualizer_module**: this module allows using the cyberith virtualizer as a joypad interface for walking commands. * **Utils_module**: an module that can be useful to implement some common functionality +The technical description of the suit is described [here](./docs/FrameDescriptions.md). + # Overview - [:orange_book: The general idea](#orange_book-some-theory-behind-the-code) - [:page_facing_up: Dependencies](#page_facing_up-dependencies) diff --git a/app/robots/iCubGazeboV2_5/XsensRetargeting.ini b/app/robots/iCubGazeboV2_5/XsensRetargeting.ini new file mode 100644 index 00000000..92902357 --- /dev/null +++ b/app/robots/iCubGazeboV2_5/XsensRetargeting.ini @@ -0,0 +1,24 @@ +name XsensRetargeting +samplingTime 0.01 +robot icub + +remote_control_boards ("head", "torso", "left_arm", "right_arm") +# Notice the order of the joint list is not wrong. +# Indeed they are written according to the joint order of the walking-coordinator +joints_list ("neck_pitch", "neck_roll", "neck_yaw", + "torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw") + +# Notice the order of the human joint list is not wrong. +# Indeed they are written according to the human joint order of the HDE/HumanStateWrapper +human_joint_list_stream ("neck_roll", "l_elbow", "l_shoulder_roll", "r_shoulder_yaw", "r_hip_yaw", "r_shoulder_pitch", "torso_roll", "l_ankle_roll", + "l_shoulder_yaw", "r_ankle_roll", "torso_pitch", "l_knee", "r_elbow", "l_hip_yaw", "neck_yaw", "r_wrist_prosup", + "r_wrist_yaw", "r_shoulder_roll", "l_shoulder_pitch", "l_hip_roll", "neck_pitch", "r_ankle_pitch", "l_wrist_prosup", "l_ankle_pitch", + "r_wrist_pitch", "r_hip_pitch", "r_knee", "l_hip_pitch", "l_wrist_yaw", "r_hip_roll", "l_wrist_pitch", "torso_yaw") + +smoothingTime 0.25 +# The max difference (threshold) of a joint value coming from the human (rad) +jointDifferenceThreshold 1.5 +wholeBodyJointsPort /HumanStateWrapper/state:i +controllerPort /jointPosition:o diff --git a/app/robots/iCubGazeboV2_5/headRetargetingParams.ini b/app/robots/iCubGazeboV2_5/headRetargetingParams.ini new file mode 100644 index 00000000..9059ee96 --- /dev/null +++ b/app/robots/iCubGazeboV2_5/headRetargetingParams.ini @@ -0,0 +1,6 @@ +remote_control_boards ("head") +# Notice the order of the joint list is not wrong. +# Indeed they are written according to the joint order of the icub-neck +joints_list ("neck_pitch", "neck_roll", "neck_yaw") + +smoothingTime 1.0 diff --git a/app/robots/iCubGazeboV2_5/leftFingersRetargetingParams.ini b/app/robots/iCubGazeboV2_5/leftFingersRetargetingParams.ini new file mode 100644 index 00000000..9307768d --- /dev/null +++ b/app/robots/iCubGazeboV2_5/leftFingersRetargetingParams.ini @@ -0,0 +1,6 @@ +remote_control_boards ("left_arm") +joints_list ("l_thumb_proximal", "l_thumb_distal", "l_index_proximal", "l_index-distal", "l_middle-proximal", "l_middle-distal", "l_little-fingers") + +useVelocity 1 + +fingersScaling (1, 3.5, 2, 3.5, 1, 3.5, 5) diff --git a/app/robots/iCubGazeboV2_5/leftHandRetargetingParams.ini b/app/robots/iCubGazeboV2_5/leftHandRetargetingParams.ini new file mode 100644 index 00000000..f074a467 --- /dev/null +++ b/app/robots/iCubGazeboV2_5/leftHandRetargetingParams.ini @@ -0,0 +1,21 @@ +# Scaling fractor dipending on the user height +# This specific scaling is evaluated for a 1.80m tall user +scalingFactor 0.63 + +# additional rotations +# the following rotation map the hand oculus frame and the hand robot frame (Identity) +handOculusFrame_R_handRobotFrame ((1.0 0.0 0.0), (0.0 1.0 0.0), (0.0 0.0 1.0)) + +# the following rotation map the teleoperation frame and the teleoperation robot +# frame the Teleoperation robot frame chosen is "imu_frame" and according to iCub +# CAD has: +# The z-axis is parallel to gravity but pointing upwards. +# The x-axis points behind the robot. +# The y-axis points laterally and is chosen according to the right-hand rule. +# the Teleoperation frame is attached to the virtualizer and the origin is the +# same oculus inertial frame. In other words: +# The z-axis is parallel to gravity but pointing upwards +# The x-axis points forward +# The y-axis points laterally and is chosen according to the right-hand rule. +# So the rotation matrix is simply given by Rotz(pi) +teleoperationRobotFrame_R_teleoperationFrame ((-1.0 0.0 0.0), (0.0 -1.0 0.0), (0.0 0.0 1.0)) diff --git a/app/robots/iCubGazeboV2_5/oculusConfig.ini b/app/robots/iCubGazeboV2_5/oculusConfig.ini new file mode 100644 index 00000000..dcd5f4b4 --- /dev/null +++ b/app/robots/iCubGazeboV2_5/oculusConfig.ini @@ -0,0 +1,44 @@ +name oculusRetargeting + +# ports +leftHandPosePort /leftHandPose:o +rightHandPosePort /rightHandPose:o +playerOrientationPort /playerOrientation:i +rpcWalkingPort_name /walkingRpc +rpcVirtualizerPort_name /virtualizerRpc + +[GENERAL] +samplingTime 0.01 +robot icub +useXsens 1 + +# include head parameters +[include HEAD_RETARGETING "headRetargetingParams.ini"] + +# include fingers parameters +[include LEFT_FINGERS_RETARGETING "leftFingersRetargetingParams.ini"] +[include RIGHT_FINGERS_RETARGETING "rightFingersRetargetingParams.ini"] + +# include hand parameters +[include LEFT_HAND_RETARGETING "leftHandRetargetingParams.ini"] +[include RIGHT_HAND_RETARGETING "rightHandRetargetingParams.ini"] + +# include Torso parameters [iff using Xsens] +[include TORSO_RETARGETING "torsoRetargeting.ini"] + +[OCULUS] +root_frame_name mobile_base_body_link +left_hand_frame_name loculus +right_hand_frame_name roculus +oculusOrientationPort /oculusOrientation:i + +move_icub_using_joypad 1 +deadzone 0.3 +fullscale 1.0 +scale_X 5.0 +scale_Y 5.0 +use_left 1 + + +# root_frame_name oculusworld +# head_frame_name headoculus diff --git a/app/robots/iCubGazeboV2_5/rightFingersRetargetingParams.ini b/app/robots/iCubGazeboV2_5/rightFingersRetargetingParams.ini new file mode 100644 index 00000000..d4af0664 --- /dev/null +++ b/app/robots/iCubGazeboV2_5/rightFingersRetargetingParams.ini @@ -0,0 +1,6 @@ +remote_control_boards ("right_arm") +joints_list ("r_thumb_proximal", "r_thumb_distal", "r_index_proximal", "r_index-distal", "r_middle-proximal", "r_middle-distal", "r_little-fingers") + +useVelocity 1 + +fingersScaling (1, 3.5, 2, 3.5, 1, 3.5, 5) diff --git a/app/robots/iCubGazeboV2_5/rightHandRetargetingParams.ini b/app/robots/iCubGazeboV2_5/rightHandRetargetingParams.ini new file mode 100644 index 00000000..f074a467 --- /dev/null +++ b/app/robots/iCubGazeboV2_5/rightHandRetargetingParams.ini @@ -0,0 +1,21 @@ +# Scaling fractor dipending on the user height +# This specific scaling is evaluated for a 1.80m tall user +scalingFactor 0.63 + +# additional rotations +# the following rotation map the hand oculus frame and the hand robot frame (Identity) +handOculusFrame_R_handRobotFrame ((1.0 0.0 0.0), (0.0 1.0 0.0), (0.0 0.0 1.0)) + +# the following rotation map the teleoperation frame and the teleoperation robot +# frame the Teleoperation robot frame chosen is "imu_frame" and according to iCub +# CAD has: +# The z-axis is parallel to gravity but pointing upwards. +# The x-axis points behind the robot. +# The y-axis points laterally and is chosen according to the right-hand rule. +# the Teleoperation frame is attached to the virtualizer and the origin is the +# same oculus inertial frame. In other words: +# The z-axis is parallel to gravity but pointing upwards +# The x-axis points forward +# The y-axis points laterally and is chosen according to the right-hand rule. +# So the rotation matrix is simply given by Rotz(pi) +teleoperationRobotFrame_R_teleoperationFrame ((-1.0 0.0 0.0), (0.0 -1.0 0.0), (0.0 0.0 1.0)) diff --git a/app/robots/iCubGazeboV2_5/torsoRetargeting.ini b/app/robots/iCubGazeboV2_5/torsoRetargeting.ini new file mode 100644 index 00000000..7f8a6f0b --- /dev/null +++ b/app/robots/iCubGazeboV2_5/torsoRetargeting.ini @@ -0,0 +1,2 @@ +remote_control_boards ("torso") +joints_list ("torso_pitch", "torso_roll", "torso_yaw") diff --git a/app/robots/iCubGenova04/XsensRetargeting.ini b/app/robots/iCubGenova04/XsensRetargeting.ini new file mode 100644 index 00000000..ce49894f --- /dev/null +++ b/app/robots/iCubGenova04/XsensRetargeting.ini @@ -0,0 +1,24 @@ +name XsensRetargeting +samplingTime 0.01 +robot icub + +remote_control_boards ("head", "torso", "left_arm", "right_arm") +# Notice the order of the joint list is not wrong. +# Indeed they are written according to the joint order of the walking-coordinator +joints_list ("neck_pitch", "neck_roll", "neck_yaw", + "torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw") + +# Notice the order of the human joint list is not wrong. +# Indeed they are written according to the human joint order of the HDE/HumanStateWrapper +human_joint_list_stream ("neck_roll", "l_elbow", "l_shoulder_roll", "r_shoulder_yaw", "r_hip_yaw", "r_shoulder_pitch", "torso_roll", "l_ankle_roll", + "l_shoulder_yaw", "r_ankle_roll", "torso_pitch", "l_knee", "r_elbow", "l_hip_yaw", "neck_yaw", "r_wrist_prosup", + "r_wrist_yaw", "r_shoulder_roll", "l_shoulder_pitch", "l_hip_roll", "neck_pitch", "r_ankle_pitch", "l_wrist_prosup", "l_ankle_pitch", + "r_wrist_pitch", "r_hip_pitch", "r_knee", "l_hip_pitch", "l_wrist_yaw", "r_hip_roll", "l_wrist_pitch", "torso_yaw") + +smoothingTime 0.25 +# The max difference (threshold) of a joint value coming from the human (rad) +jointDifferenceThreshold 0.5 +wholeBodyJointsPort /HumanStateWrapper/state:i +controllerPort /jointPosition:o diff --git a/app/robots/iCubGenova04/oculusConfig.ini b/app/robots/iCubGenova04/oculusConfig.ini index a549cf2f..dcd5f4b4 100644 --- a/app/robots/iCubGenova04/oculusConfig.ini +++ b/app/robots/iCubGenova04/oculusConfig.ini @@ -8,8 +8,9 @@ rpcWalkingPort_name /walkingRpc rpcVirtualizerPort_name /virtualizerRpc [GENERAL] -samplingTime 0.05 +samplingTime 0.01 robot icub +useXsens 1 # include head parameters [include HEAD_RETARGETING "headRetargetingParams.ini"] @@ -22,6 +23,9 @@ robot icub [include LEFT_HAND_RETARGETING "leftHandRetargetingParams.ini"] [include RIGHT_HAND_RETARGETING "rightHandRetargetingParams.ini"] +# include Torso parameters [iff using Xsens] +[include TORSO_RETARGETING "torsoRetargeting.ini"] + [OCULUS] root_frame_name mobile_base_body_link left_hand_frame_name loculus diff --git a/app/robots/iCubGenova04/torsoRetargeting.ini b/app/robots/iCubGenova04/torsoRetargeting.ini new file mode 100644 index 00000000..7f8a6f0b --- /dev/null +++ b/app/robots/iCubGenova04/torsoRetargeting.ini @@ -0,0 +1,2 @@ +remote_control_boards ("torso") +joints_list ("torso_pitch", "torso_roll", "torso_yaw") diff --git a/app/robots/icubGazeboSim/XsensRetargeting.ini b/app/robots/icubGazeboSim/XsensRetargeting.ini new file mode 100644 index 00000000..b69b6954 --- /dev/null +++ b/app/robots/icubGazeboSim/XsensRetargeting.ini @@ -0,0 +1,24 @@ +name XsensRetargeting +samplingTime 0.01 +robot icubSim + +remote_control_boards ("head", "torso", "left_arm", "right_arm") +# Notice the order of the joint list is not wrong. +# Indeed they are written according to the joint order of the walking-coordinator +joints_list ("neck_pitch", "neck_roll", "neck_yaw", + "torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw") + +# Notice the order of the human joint list is not wrong. +# Indeed they are written according to the human joint order of the HDE/HumanStateWrapper +human_joint_list_stream ("neck_roll", "l_elbow", "l_shoulder_roll", "r_shoulder_yaw", "r_hip_yaw", "r_shoulder_pitch", "torso_roll", "l_ankle_roll", + "l_shoulder_yaw", "r_ankle_roll", "torso_pitch", "l_knee", "r_elbow", "l_hip_yaw", "neck_yaw", "r_wrist_prosup", + "r_wrist_yaw", "r_shoulder_roll", "l_shoulder_pitch", "l_hip_roll", "neck_pitch", "r_ankle_pitch", "l_wrist_prosup", "l_ankle_pitch", + "r_wrist_pitch", "r_hip_pitch", "r_knee", "l_hip_pitch", "l_wrist_yaw", "r_hip_roll", "l_wrist_pitch", "torso_yaw") + +smoothingTime 0.25 +# The max difference (threshold) of a joint value coming from the human (rad) +jointDifferenceThreshold 1.5 +wholeBodyJointsPort /HumanStateWrapper/state:i +controllerPort /jointPosition:o diff --git a/app/scripts/dcmWalkingRetargeting.xml b/app/scripts/dcmWalkingRetargeting.xml index 2c43ba8a..b5ad5587 100644 --- a/app/scripts/dcmWalkingRetargeting.xml +++ b/app/scripts/dcmWalkingRetargeting.xml @@ -54,6 +54,7 @@ OculusRetargetingModule icub-head + --useXsens 0 diff --git a/app/scripts/dcmWalkingRetargetingVirtualizer.xml b/app/scripts/dcmWalkingRetargetingVirtualizer.xml index 3c987a81..4a440f39 100644 --- a/app/scripts/dcmWalkingRetargetingVirtualizer.xml +++ b/app/scripts/dcmWalkingRetargetingVirtualizer.xml @@ -58,7 +58,7 @@ OculusRetargetingModule icub-head - --OCULUS::move_icub_using_joypad 0 + --OCULUS::move_icub_using_joypad 0 --useXsens 0 diff --git a/app/scripts/dcmWalkingRetargetingVirtualizerXsens.xml b/app/scripts/dcmWalkingRetargetingVirtualizerXsens.xml new file mode 100644 index 00000000..c0f6e5ac --- /dev/null +++ b/app/scripts/dcmWalkingRetargetingVirtualizerXsens.xml @@ -0,0 +1,166 @@ + + + + + + + DCM walking retargeting (Virtualizer Xsens) + 2D-DCM walking application. + 1.0 + + Kourosh Darvish + Giulio Romualdi + Yue Hu + + + + yarpdev + --from camera/ServerGrabberDualDragon.ini --split true + icub-head + + + + camCalibWithPose + --context cameraCalibration --from icubEyes.ini --group CAMERA_CALIBRATION_LEFT --name /icub/camcalib/left --useIMU + icub-virtualizer + + + + camCalibWithPose + --context cameraCalibration --from icubEyes.ini --group CAMERA_CALIBRATION_RIGHT --name /icub/camcalib/right --useIMU + icub-virtualizer + + + + yarpdev + --device transformServer --ROS::enable_ros_publisher 0 --ROS::enable_ros_subscriber 0 + icub-virtualizer + + + + yarpdev + --device JoypadControlServer --use_separate_ports 1 --period 10 --name /joypadDevice/Oculus --subdevice ovrheadset --sticks 0 --tfDevice transformClient --tfLocal /oculustf --tfRemote /transformServer --tf_head_frame headoculus --tf_left_hand_frame loculus --tf_right_hand_frame roculus --tf_root_frame oculusworld --stick_as_axis true --gui_elements 0 + icub-virtualizer + + + + + + WalkingModule + icub-head + + + + VirtualizerModule + icub-virtualizer + --context virtualizerRetargeting + + + + OculusRetargetingModule + icub-head + --OCULUS::move_icub_using_joypad 0 + + + + + XsensRetargetingModule + icub-head + + + + + + + + /walking-coordinator/torsoYaw:o + /virtualizer/robotOrientation:i + + + + /walking-coordinator/torsoYaw:o + /oculusRetargeting/robotOrientation:i + + + + /oculusRetargeting/walkingRpc + /walking-coordinator/rpc + + + + /oculusRetargeting/virtualizerRpc + /virtualizer/rpc + + + + /virtualizer/walkingRpc + /walking-coordinator/rpc + + + + /virtualizer/playerOrientation:o + /oculusRetargeting/playerOrientation:i + + + + + /icub/cam/left + /icub/camcalib/left/in + mjpeg + + + + /icub/cam/right + /icub/camcalib/right/in + mjpeg + + + + /icub/head/state:o + /icub/camcalib/left/head_encs/in + udp + + + + /icub/head/state:o + /icub/camcalib/right/head_encs/in + udp + + + + + /oculusRetargeting/imagesOrientation:o + /icub/camcalib/left/imu/in + udp + + + + + /oculusRetargeting/imagesOrientation:o + /icub/camcalib/right/imu/in + udp + + + + /icub/camcalib/left/out + /oculus/display/left:i + shmem + + + + /icub/camcalib/right/out + /oculus/display/right:i + shmem + + + + /HDE/RobotStateWrapper/state:o + /XsensRetargeting/HumanStateWrapper/state:i + + + + /XsensRetargeting/jointPosition:o + /walking-coordinator/jointPosition:i + + + diff --git a/app/scripts/dcmWalkingRetargetingXsens.xml b/app/scripts/dcmWalkingRetargetingXsens.xml new file mode 100644 index 00000000..fd09d68b --- /dev/null +++ b/app/scripts/dcmWalkingRetargetingXsens.xml @@ -0,0 +1,121 @@ + + + + + + + DCM walking retargeting (Xsens) + 2D-DCM walking application. + 1.0 + + Kourosh Darvish + Giulio Romualdi + Yue Hu + + + + + + yarpdev + --from camera/ServerGrabberDualDragon.ini --split true + icub-head + + + + camCalibWithPose + --context cameraCalibration --from icubEyes.ini --group CAMERA_CALIBRATION_LEFT --name /icub/camcalib/left + icub-virtualizer + + + + camCalibWithPose + --context cameraCalibration --from icubEyes.ini --group CAMERA_CALIBRATION_RIGHT --name /icub/camcalib/right + icub-virtualizer + + + + yarpdev + --device transformServer --ROS::enable_ros_publisher 0 --ROS::enable_ros_subscriber 0 + icub-virtualizer + + + + yarpdev + --device JoypadControlServer --use_separate_ports 1 --period 10 --name /joypadDevice/Oculus --subdevice ovrheadset --sticks 0 --tfDevice transformClient --tfLocal /oculustf --tfRemote /transformServer --tf_left_hand_frame loculus --tf_right_hand_frame roculus --tf_root_frame oculusworld --stick_as_axis true --gui_elements 0 + icub-virtualizer + + + + + + WalkingModule + icub-head + + + + OculusRetargetingModule + icub-head + + + + + XsensRetargetingModule + icub-head + + + + + + /oculusRetargeting/walkingRpc + /walking-coordinator/rpc + + + + + /icub/cam/left + /icub/camcalib/left/in + mjpeg + + + + /icub/cam/right + /icub/camcalib/right/in + mjpeg + + + + /icub/head/state:o + /icub/camcalib/left/head_encs/in + udp + + + + /icub/head/state:o + /icub/camcalib/right/head_encs/in + udp + + + + /icub/camcalib/left/out + /oculus/display/left:i + shmem + + + + /icub/camcalib/right/out + /oculus/display/right:i + shmem + + + + + /HDE/RobotStateWrapper/state:o + /XsensRetargeting/HumanStateWrapper/state:i + + + + /XsensRetargeting/jointPosition:o + /walking-coordinator/jointPosition:i + + + diff --git a/docs/FrameDescriptions.md b/docs/FrameDescriptions.md new file mode 100644 index 00000000..b646cbf0 --- /dev/null +++ b/docs/FrameDescriptions.md @@ -0,0 +1,21 @@ +Following are the descriptions of the Frames used by different devices in our framework following these issues: + +**Oculus Device** + +

+ +

+ +**Virtualizer (treadmill)** + +

+ +

+ +**Xsens Suit** + +

+ +

|

+ +

diff --git a/docs/images/Oculus.png b/docs/images/Oculus.png new file mode 100644 index 00000000..d6c35c73 Binary files /dev/null and b/docs/images/Oculus.png differ diff --git a/docs/images/RobotFrames.png b/docs/images/RobotFrames.png new file mode 100644 index 00000000..6dc63a4a Binary files /dev/null and b/docs/images/RobotFrames.png differ diff --git a/docs/images/Virtualizer+others.jpg b/docs/images/Virtualizer+others.jpg new file mode 100644 index 00000000..6acb2730 Binary files /dev/null and b/docs/images/Virtualizer+others.jpg differ diff --git a/docs/images/humanXses.png b/docs/images/humanXses.png new file mode 100644 index 00000000..4ddd1de2 Binary files /dev/null and b/docs/images/humanXses.png differ diff --git a/docs/images/virtualizer+oculus].jpg b/docs/images/virtualizer+oculus].jpg new file mode 100644 index 00000000..7a62f543 Binary files /dev/null and b/docs/images/virtualizer+oculus].jpg differ diff --git a/modules/CMakeLists.txt b/modules/CMakeLists.txt index 1e180af5..d76bc67e 100644 --- a/modules/CMakeLists.txt +++ b/modules/CMakeLists.txt @@ -4,6 +4,7 @@ add_subdirectory(Utils) add_subdirectory(Oculus_module) +add_subdirectory(Xsens_module) if (MSVC) add_subdirectory(Virtualizer_module) endif (MSVC) diff --git a/modules/Oculus_module/CMakeLists.txt b/modules/Oculus_module/CMakeLists.txt index 96f517d9..0085b427 100644 --- a/modules/Oculus_module/CMakeLists.txt +++ b/modules/Oculus_module/CMakeLists.txt @@ -29,7 +29,9 @@ set(${EXE_TARGET_NAME}_SRC src/HeadRetargeting.cpp src/RobotControlHelper.cpp src/RetargetingController.cpp - src/OculusModule.cpp) + src/OculusModule.cpp + src/TorsoRetargeting.cpp + ) # set hpp files set(${EXE_TARGET_NAME}_HDR @@ -38,7 +40,9 @@ set(${EXE_TARGET_NAME}_HDR include/HeadRetargeting.hpp include/RobotControlHelper.hpp include/RetargetingController.hpp - include/OculusModule.hpp) + include/OculusModule.hpp + include/TorsoRetargeting.hpp + ) # add include directories to the build. include_directories( diff --git a/modules/Oculus_module/include/OculusModule.hpp b/modules/Oculus_module/include/OculusModule.hpp index 88c0543f..73f0d8c2 100644 --- a/modules/Oculus_module/include/OculusModule.hpp +++ b/modules/Oculus_module/include/OculusModule.hpp @@ -26,6 +26,7 @@ #include #include #include +#include /** * OculusModule is the main core of the Oculus application. It is goal is to evaluate retrieve the @@ -67,6 +68,7 @@ class OculusModule : public yarp::os::RFModule unsigned int m_prepareWalkingIndex; /**< Index of the prepare walking button */ bool m_useVirtualizer; /**< True if the virtualizer is used in the retargeting */ + bool m_useXsens; /**< True if the Xsens is used in the retargeting */ // transform server yarp::dev::PolyDriver m_transformClientDevice; /**< Transform client. */ @@ -93,6 +95,8 @@ class OculusModule : public yarp::os::RFModule std::unique_ptr m_leftHand; /**< Pointer to the left hand retargeting object. */ + std::unique_ptr m_torso; /**< Pointer to the torso retargeting object. */ + // ports yarp::os::BufferedPort m_leftHandPosePort; /**< Left hand port pose. */ yarp::os::BufferedPort m_rightHandPosePort; /**< Right hand port pose. */ @@ -111,6 +115,12 @@ class OculusModule : public yarp::os::RFModule yarp::os::RpcClient m_rpcVirtualizerClient; /**< Rpc client used for sending command to the virtualizer */ + /** Port used to retrieve the human whole body joint pose. */ + yarp::os::BufferedPort m_wholeBodyHumanJointsPort; + + /** Port used to retrieve the human whole body joint pose. */ + yarp::os::BufferedPort m_wholeBodyHumanSmoothedJointsPort; + double m_robotYaw; /**< Yaw angle of the robot base */ yarp::sig::Matrix m_oculusRoot_T_lOculus; diff --git a/modules/Oculus_module/include/RetargetingController.hpp b/modules/Oculus_module/include/RetargetingController.hpp index 98de118d..c195b4ec 100644 --- a/modules/Oculus_module/include/RetargetingController.hpp +++ b/modules/Oculus_module/include/RetargetingController.hpp @@ -54,5 +54,6 @@ class RetargetingController * @return control helper interface */ std::unique_ptr& controlHelper(); + virtual ~RetargetingController(); }; #endif diff --git a/modules/Oculus_module/include/TorsoRetargeting.hpp b/modules/Oculus_module/include/TorsoRetargeting.hpp new file mode 100644 index 00000000..373f9de0 --- /dev/null +++ b/modules/Oculus_module/include/TorsoRetargeting.hpp @@ -0,0 +1,57 @@ +/** + * @file HeadRetargeting.hpp + * @authors Kourosh Darvish + * Giulio Romualdi + * Mohamed Babiker Mohamed Elobaid + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +#ifndef TORSO_RETARGETING_HPP +#define TORSO_RETARGETING_HPP + +// std +#include + +// YARP +#include + +// iCub-ctrl +#include + +// iDynTree +#include + +#include + +/** + * Class useful to get torso info for retargeting. + */ +class TorsoRetargeting : public RetargetingController +{ +public: + /** + * Configure the object. + * @param config is the reference to a resource finder object. + * @param name is the name of the robot. + * @return true in case of success and false otherwise. + */ + bool configure(const yarp::os::Searchable& config, const std::string& name) override; + + /** + * Evaluate the forward kinematics of the torso + * Further details on the joints name can be found in + * http://wiki.icub.org/wiki/ICub_Model_naming_conventions#Joints + * @param torsoPitch neck pitch angle expressed in radiant + * @param torsoRoll neck roll angle expressed in radiant + * @param torsoYaw neck yaw angle expressed in radiant + * @return rotation matrix of the chest with respect to the root_link frame + */ + static iDynTree::Rotation + forwardKinematics(const double& torsoPitch, const double& torsoRoll, const double& torsoYaw); + + bool move() override; +}; + +#endif diff --git a/modules/Oculus_module/src/OculusModule.cpp b/modules/Oculus_module/src/OculusModule.cpp index 5505b3c3..6abb7837 100644 --- a/modules/Oculus_module/src/OculusModule.cpp +++ b/modules/Oculus_module/src/OculusModule.cpp @@ -195,6 +195,9 @@ bool OculusModule::configure(yarp::os::ResourceFinder& rf) } setName(name.c_str()); + m_useXsens = generalOptions.check("useXsens", yarp::os::Value(false)).asBool(); + yInfo() << "Teleoperation uses Xsens: " << m_useXsens; + yarp::os::Bottle& oculusOptions = rf.findGroup("OCULUS"); if (!configureOculus(oculusOptions)) { @@ -212,6 +215,22 @@ bool OculusModule::configure(yarp::os::ResourceFinder& rf) return false; } + // configure torso retargeting, iff we use Xsens + + yInfo() << "[OculusModule::configure] initialize the torso!"; + if (m_useXsens) + { + m_torso = std::make_unique(); + yarp::os::Bottle& torsoOptions = rf.findGroup("TORSO_RETARGETING"); + torsoOptions.append(generalOptions); + if (!m_torso->configure(torsoOptions, getName())) + { + yError() << "[OculusModule::configure] Unable to initialize the torso retargeting."; + return false; + } + } + yInfo() << "[OculusModule::configure] finish the torso!"; + // configure fingers retargeting m_leftHandFingers = std::make_unique(); yarp::os::Bottle& leftFingersOptions = rf.findGroup("LEFT_FINGERS_RETARGETING"); @@ -338,6 +357,11 @@ bool OculusModule::close() m_rightHandFingers->controlHelper()->close(); m_leftHandFingers->controlHelper()->close(); + if (m_useXsens) + { + m_torso->controlHelper()->close(); + } + m_joypadDevice.close(); m_transformClientDevice.close(); @@ -361,83 +385,100 @@ double OculusModule::evaluateDesiredFingersVelocity(unsigned int squeezeIndex, bool OculusModule::getTransforms() { - // check if everything is ok - if (!m_frameTransformInterface->frameExists(m_rootFrameName)) + if (!m_useXsens) { - yError() << "[OculusModule::getTransforms] No " << m_rootFrameName << " frame."; - return false; - } + // check if everything is ok + if (!m_frameTransformInterface->frameExists(m_rootFrameName)) + { + yError() << "[OculusModule::getTransforms] No " << m_rootFrameName << " frame."; + return false; + } - if (!m_frameTransformInterface->frameExists(m_headFrameName)) - { - // head - yarp::os::Bottle* desiredHeadOrientation = NULL; + if (!m_frameTransformInterface->frameExists(m_headFrameName)) + { + // head + yarp::os::Bottle* desiredHeadOrientation = NULL; - iDynTree::Vector3 desiredHeadOrientationVector; - desiredHeadOrientation = m_oculusOrientationPort.read(false); - if (desiredHeadOrientation != NULL) + iDynTree::Vector3 desiredHeadOrientationVector; + desiredHeadOrientation = m_oculusOrientationPort.read(false); + if (desiredHeadOrientation != NULL) + { + for (int i = 0; i < desiredHeadOrientation->size(); i++) + desiredHeadOrientationVector(i) + = iDynTree::deg2rad(desiredHeadOrientation->get(i).asDouble()); + + // Notice that the data coming from the port are written in the following order: + // [ pitch, -roll, yaw]. + iDynTree::toEigen(m_oculusRoot_T_headOculus).block(0, 0, 3, 3) + = iDynTree::toEigen(iDynTree::Rotation::RPY(-desiredHeadOrientationVector(1), + desiredHeadOrientationVector(0), + desiredHeadOrientationVector(2))); + } + } else { - for (int i = 0; i < desiredHeadOrientation->size(); i++) - desiredHeadOrientationVector(i) - = iDynTree::deg2rad(desiredHeadOrientation->get(i).asDouble()); - - // Notice that the data coming from the port are written in the following order: - // [ pitch, -roll, yaw]. - iDynTree::toEigen(m_oculusRoot_T_headOculus).block(0, 0, 3, 3) - = iDynTree::toEigen(iDynTree::Rotation::RPY(-desiredHeadOrientationVector(1), - desiredHeadOrientationVector(0), - desiredHeadOrientationVector(2))); + if (!m_frameTransformInterface->getTransform( + m_headFrameName, m_rootFrameName, m_oculusRoot_T_headOculus)) + { + yError() << "[OculusModule::getTransforms] Unable to evaluate the " + << m_headFrameName << " to " << m_rootFrameName << "transformation"; + return false; + } } - } else - { - if (!m_frameTransformInterface->getTransform( - m_headFrameName, m_rootFrameName, m_oculusRoot_T_headOculus)) + + if (!m_frameTransformInterface->frameExists(m_leftHandFrameName)) { - yError() << "[OculusModule::getTransforms] Unable to evaluate the " << m_headFrameName - << " to " << m_rootFrameName << "transformation"; + + yError() << "[OculusModule::getTransforms] No " << m_leftHandFrameName << " frame."; return false; } - } - if (!m_frameTransformInterface->frameExists(m_leftHandFrameName)) - { - yError() << "[OculusModule::getTransforms] No " << m_leftHandFrameName << " frame."; - return false; - } - - if (!m_frameTransformInterface->frameExists(m_rightHandFrameName)) - { - yError() << "[OculusModule::getTransforms] No " << m_rightHandFrameName << " frame."; - return false; - } + if (!m_frameTransformInterface->frameExists(m_rightHandFrameName)) + { + yError() << "[OculusModule::getTransforms] No " << m_rightHandFrameName << " frame."; + return false; + } - if (!m_frameTransformInterface->getTransform( - m_leftHandFrameName, m_rootFrameName, m_oculusRoot_T_lOculus)) - { - yError() << "[OculusModule::getTransforms] Unable to evaluate the " << m_leftHandFrameName - << " to " << m_rootFrameName << "transformation"; - return false; - } + if (!m_frameTransformInterface->getTransform( + m_leftHandFrameName, m_rootFrameName, m_oculusRoot_T_lOculus)) + { + yError() << "[OculusModule::getTransforms] Unable to evaluate the " + << m_leftHandFrameName << " to " << m_rootFrameName << "transformation"; + return false; + } - if (!m_frameTransformInterface->getTransform( - m_rightHandFrameName, m_rootFrameName, m_oculusRoot_T_rOculus)) - { - yError() << "[OculusModule::getTransforms] Unable to evaluate the " << m_rightHandFrameName - << " to " << m_rootFrameName << "transformation"; - return false; + if (!m_frameTransformInterface->getTransform( + m_rightHandFrameName, m_rootFrameName, m_oculusRoot_T_rOculus)) + { + yError() << "[OculusModule::getTransforms] Unable to evaluate the " + << m_rightHandFrameName << " to " << m_rootFrameName << "transformation"; + return false; + } } - return true; } bool OculusModule::getFeedbacks() { + + if (m_useXsens) + { + if (!m_torso->controlHelper()->getFeedback()) + { + yError() << "[OculusModule::getFeedbacks] Unable to get the joint encoders feedback: " + "TorsoRetargeting"; + return false; + } + m_torso->controlHelper()->updateTimeStamp(); + } + if (!m_head->controlHelper()->getFeedback()) { - yError() << "[OculusModule::getFeedbacks] Unable to get the joint encoders feedback"; + yError() << "[OculusModule::getFeedbacks] Unable to get the joint encoders feedback: " + "HeadRetargeting"; return false; } m_head->controlHelper()->updateTimeStamp(); + return true; } @@ -471,14 +512,49 @@ bool OculusModule::updateModule() m_robotYaw = Angles::normalizeAngle((*robotOrientation)(0)); } - m_head->setPlayerOrientation(m_playerOrientation); - m_head->setDesiredHeadOrientation(m_oculusRoot_T_headOculus); - // m_head->setDesiredHeadOrientation(desiredHeadOrientationVector(0), - // desiredHeadOrientationVector(1), desiredHeadOrientationVector(2)); - if (!m_head->move()) + if (!m_useXsens) { - yError() << "[updateModule::updateModule] unable to move the head"; - return false; + m_head->setPlayerOrientation(m_playerOrientation); + m_head->setDesiredHeadOrientation(m_oculusRoot_T_headOculus); + // m_head->setDesiredHeadOrientation(desiredHeadOrientationVector(0), + // desiredHeadOrientationVector(1), desiredHeadOrientationVector(2)); + if (!m_head->move()) + { + yError() << "[OculusModule::updateModule] unable to move the head"; + return false; + } + + // left hand + yarp::sig::Vector& leftHandPose = m_leftHandPosePort.prepare(); + m_leftHand->setPlayerOrientation(m_playerOrientation); + m_leftHand->setHandTransform(m_oculusRoot_T_lOculus); + m_leftHand->evaluateDesiredHandPose(leftHandPose); + m_leftHandPosePort.write(); + + // right hand + yarp::sig::Vector& rightHandPose = m_rightHandPosePort.prepare(); + m_rightHand->setPlayerOrientation(m_playerOrientation); + m_rightHand->setHandTransform(m_oculusRoot_T_rOculus); + m_rightHand->evaluateDesiredHandPose(rightHandPose); + m_rightHandPosePort.write(); + } + + // use joypad + if (!m_useVirtualizer) + { + yarp::os::Bottle cmd, outcome; + double x, y; + m_joypadControllerInterface->getAxis(m_xJoypadIndex, x); + m_joypadControllerInterface->getAxis(m_yJoypadIndex, y); + + x = -m_scaleX * deadzone(x); + y = m_scaleY * deadzone(y); + std::swap(x, y); + + cmd.addString("setGoal"); + cmd.addDouble(x); + cmd.addDouble(y); + m_rpcWalkingClient.write(cmd, outcome); } // left fingers @@ -509,37 +585,6 @@ bool OculusModule::updateModule() return false; } - // left hand - yarp::sig::Vector& leftHandPose = m_leftHandPosePort.prepare(); - m_leftHand->setPlayerOrientation(m_playerOrientation); - m_leftHand->setHandTransform(m_oculusRoot_T_lOculus); - m_leftHand->evaluateDesiredHandPose(leftHandPose); - m_leftHandPosePort.write(); - - // right hand - yarp::sig::Vector& rightHandPose = m_rightHandPosePort.prepare(); - m_rightHand->setPlayerOrientation(m_playerOrientation); - m_rightHand->setHandTransform(m_oculusRoot_T_rOculus); - m_rightHand->evaluateDesiredHandPose(rightHandPose); - m_rightHandPosePort.write(); - - // use joypad - if (!m_useVirtualizer) - { - yarp::os::Bottle cmd, outcome; - double x, y; - m_joypadControllerInterface->getAxis(m_xJoypadIndex, x); - m_joypadControllerInterface->getAxis(m_yJoypadIndex, y); - - x = -m_scaleX * deadzone(x); - y = m_scaleY * deadzone(y); - std::swap(x, y); - - cmd.addString("setGoal"); - cmd.addDouble(x); - cmd.addDouble(y); - m_rpcWalkingClient.write(cmd, outcome); - } } else if (m_state == OculusFSM::Configured) { // check if it is time to prepare or start walking @@ -582,12 +627,24 @@ bool OculusModule::updateModule() neckPitch = neckEncoders(0); neckRoll = neckEncoders(1); neckYaw = neckEncoders(2); - iDynTree::Rotation root_R_head + + iDynTree::Rotation chest_R_head = HeadRetargeting::forwardKinematics(neckPitch, neckRoll, neckYaw); + + iDynTree::Rotation root_R_chest = iDynTree::Rotation::Identity(); + if (m_useXsens) + { + double torsoPitch, torsoRoll, torsoYaw; + yarp::sig::Vector torsoEncoders = m_torso->controlHelper()->jointEncoders(); + torsoPitch = torsoEncoders(0); + torsoRoll = torsoEncoders(1); + torsoYaw = torsoEncoders(2); + root_R_chest = TorsoRetargeting::forwardKinematics(torsoPitch, torsoRoll, torsoYaw); + } iDynTree::Rotation inertial_R_root = iDynTree::Rotation::RotZ(-m_playerOrientation); // inertial_R_head is used to simulate an imu required by the cam calibration application - iDynTree::Rotation inertial_R_head = inertial_R_root * root_R_head; + iDynTree::Rotation inertial_R_head = inertial_R_root * root_R_chest * chest_R_head; iDynTree::Vector3 inertial_R_headRPY = inertial_R_head.asRPY(); imagesOrientation.addDouble(iDynTree::rad2deg(inertial_R_headRPY(0))); diff --git a/modules/Oculus_module/src/RetargetingController.cpp b/modules/Oculus_module/src/RetargetingController.cpp index 8ef128f4..13f06414 100644 --- a/modules/Oculus_module/src/RetargetingController.cpp +++ b/modules/Oculus_module/src/RetargetingController.cpp @@ -22,3 +22,5 @@ std::unique_ptr& RetargetingController::controlHelper() { return m_controlHelper; } + +RetargetingController::~RetargetingController(){} diff --git a/modules/Oculus_module/src/TorsoRetargeting.cpp b/modules/Oculus_module/src/TorsoRetargeting.cpp new file mode 100644 index 00000000..5f69f6b5 --- /dev/null +++ b/modules/Oculus_module/src/TorsoRetargeting.cpp @@ -0,0 +1,52 @@ +/** + * @file HeadRetargeting.cpp + * @authors Giulio Romualdi + * Mohamed Babiker Mohamed Elobaid + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +// iDynTree +#include +#include +#include + +#include +#include + +iDynTree::Rotation TorsoRetargeting::forwardKinematics(const double& torsoPitch, + const double& torsoRoll, + const double& torsoYaw) +{ + iDynTree::Rotation chest_R_root; + chest_R_root = iDynTree::Rotation::RotY(-torsoPitch) * iDynTree::Rotation::RotX(torsoRoll) + * iDynTree::Rotation::RotZ(-torsoYaw); + + return chest_R_root; +} + +bool TorsoRetargeting::configure(const yarp::os::Searchable& config, const std::string& name) +{ + // check if the configuration file is empty + if (config.isNull()) + { + yError() << "[TorsoRetargeting::configure] Empty configuration for torso retargeting."; + return false; + } + + m_controlHelper = std::make_unique(); + if (!m_controlHelper->configure(config, name, true)) + { + yError() << "[TorsoRetargeting::configure] Unable to configure the torso helper"; + return false; + } + + return true; +} + +bool TorsoRetargeting::move() +{ + yInfo() << "Nothing Implemented!"; + return true; +} diff --git a/modules/Xsens_module/CMakeLists.txt b/modules/Xsens_module/CMakeLists.txt new file mode 100644 index 00000000..5283b49d --- /dev/null +++ b/modules/Xsens_module/CMakeLists.txt @@ -0,0 +1,47 @@ +# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT) +# All Rights Reserved. +# Authors: Giulio Romualdi + +# set target name +set(EXE_TARGET_NAME XsensRetargetingModule) + +option(ENABLE_RPATH "Enable RPATH for this library" ON) +mark_as_advanced(ENABLE_RPATH) +include(AddInstallRPATHSupport) +add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}" + LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}" + INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}" + DEPENDS ENABLE_RPATH + USE_LINK_PATH) + +# Find required package +find_package(ICUB REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(iDynTree REQUIRED) +include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) +include(FindPackageHandleStandardArgs) + +# set cpp files +set(${EXE_TARGET_NAME}_SRC + src/main.cpp + src/XsensRetargeting.cpp) + +# set hpp files +set(${EXE_TARGET_NAME}_HDR + include/XsensRetargeting.hpp) + +# add include directories to the build. +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +# add an executable to the project using the specified source files. +add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC} ${${EXE_TARGET_NAME}_HDR}) + +target_link_libraries(${EXE_TARGET_NAME} LINK_PUBLIC + ${YARP_LIBRARIES} + ${iDynTree_LIBRARIES} + ctrlLib + UtilityLibrary) + +install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin) diff --git a/modules/Xsens_module/include/XsensRetargeting.hpp b/modules/Xsens_module/include/XsensRetargeting.hpp new file mode 100644 index 00000000..6c694bdc --- /dev/null +++ b/modules/Xsens_module/include/XsensRetargeting.hpp @@ -0,0 +1,103 @@ +#ifndef XSENSRETARGETING_H +#define XSENSRETARGETING_H + +// std +#include +#include + +// YARP +#include + +// iCub-ctrl +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +// iDynTree +#include +//#include + +class mapJoints +{ +public: + std::string name; + int index; +}; + +class XsensRetargeting : public yarp::os::RFModule +{ +private: + /** Minimum jerk trajectory smoother for the desired whole body joints */ + std::unique_ptr m_WBTrajectorySmoother{nullptr}; + yarp::sig::Vector m_jointValues, m_smoothedJointValues; + std::vector + m_humanJointsListName; // the order of joints list arrived from human state provider is + // different from the one we want to send to the controller + + /** Port used to retrieve the human whole body joint pose. */ + yarp::os::BufferedPort m_wholeBodyHumanJointsPort; + + /** Port used to retrieve the human whole body joint pose. */ + yarp::os::BufferedPort m_wholeBodyHumanSmoothedJointsPort; + + double m_dT; /**< Module period. */ + bool m_useXsens; /**< True if the Xsens is used in the retargeting */ + + std::vector + m_robotJointsListNames; /**< Vector containing the name of the controlled joints.*/ + size_t m_actuatedDOFs; /**< Number of the actuated DoF */ + + std::vector m_humanToRobotMap; + + bool m_firstIteration; + double m_jointDiffThreshold; + +public: + XsensRetargeting(); + ~XsensRetargeting(); + /* + * Configure the whole body retargeting retargeting. + * @param config reference to a resource finder object. + * @return true in case of success and false otherwise + */ + bool configure(const yarp::os::Searchable& config, const std::string& name); + + bool getJointValues(); + + bool getSmoothedJointValues(yarp::sig::Vector& smoothedJointValues); + + /** + * Get the period of the RFModule. + * @return the period of the module. + */ + double getPeriod() final; + + /** + * Main function of the RFModule. + * @return true in case of success and false otherwise. + */ + bool updateModule() final; + + /** + * Configure the RFModule. + * @param rf is the reference to a resource finder object + * @return true in case of success and false otherwise. + */ + bool configure(yarp::os::ResourceFinder& rf) final; + + /** + * Close the RFModule. + * @return true in case of success and false otherwise. + */ + bool close() final; +}; + +#endif // WHOLEBODYRETARGETING_H diff --git a/modules/Xsens_module/src/XsensRetargeting.cpp b/modules/Xsens_module/src/XsensRetargeting.cpp new file mode 100644 index 00000000..a4d4bddd --- /dev/null +++ b/modules/Xsens_module/src/XsensRetargeting.cpp @@ -0,0 +1,260 @@ +#include +#include +#include +#include + +XsensRetargeting::XsensRetargeting(){}; + +XsensRetargeting::~XsensRetargeting(){}; + +bool XsensRetargeting::configure(yarp::os::ResourceFinder& rf) +{ + // check if the configuration file is empty + if (rf.isNull()) + { + yError() << "[XsensRetargeting::configure] Empty configuration for the OculusModule " + "application."; + return false; + } + + // get the period + m_dT = rf.check("samplingTime", yarp::os::Value(0.1)).asDouble(); + + // set the module name + std::string name; + if (!YarpHelper::getStringFromSearchable(rf, "name", name)) + { + yError() << "[XsensRetargeting::configure] Unable to get a string from a searchable"; + return false; + } + setName(name.c_str()); + + // initialize minimum jerk trajectory for the whole body + + double smoothingTime; + if (!YarpHelper::getDoubleFromSearchable(rf, "smoothingTime", smoothingTime)) + { + yError() << "[XsensRetargeting::configure] Unable to find the whole body smoothing time"; + return false; + } + + yarp::os::Value* axesListYarp; + if (!rf.check("joints_list", axesListYarp)) + { + yError() << "[XsensRetargeting::configure] Unable to find joints_list into config file."; + return false; + } + + if (!YarpHelper::yarpListToStringVector(axesListYarp, m_robotJointsListNames)) + { + yError() << "[XsensRetargeting::configure] Unable to convert yarp list into a " + "vector of strings."; + return false; + } + m_actuatedDOFs = m_robotJointsListNames.size(); + + m_WBTrajectorySmoother + = std::make_unique(m_actuatedDOFs, m_dT, smoothingTime); + yarp::sig::Vector buff(m_actuatedDOFs, 0.0); + + m_WBTrajectorySmoother->init(buff); + m_jointValues.resize(m_actuatedDOFs, 0.0); + + yInfo() << "XsensRetargeting::configure: smoothingTime: " << smoothingTime; + yInfo() << "XsensRetargeting::configure: NoOfJoints: " << m_actuatedDOFs; + + // check the human joints name list + yarp::os::Value* humanAxesListYarp; + if (!rf.check("human_joint_list_stream", humanAxesListYarp)) + { + yError() << "[XsensRetargeting::configure] Unable to find human_joint_list_stream into " + "config file."; + return false; + } + + if (!YarpHelper::yarpListToStringVector(humanAxesListYarp, m_humanJointsListName)) + { + yError() << "[XsensRetargeting::configure] Unable to convert yarp list into a " + "vector of strings."; + return false; + } + + yInfo() << "Human joints name list: [human joints list] [robot joints list]" + << m_humanJointsListName.size() << " , " << m_robotJointsListNames.size(); + + for (size_t i = 0; i < m_humanJointsListName.size(); i++) + { + if (i < m_robotJointsListNames.size()) + yInfo() << "(" << i << "): " << m_humanJointsListName[i] << " , " + << m_robotJointsListNames[i]; + else + { + yInfo() << "(" << i << "): " << m_humanJointsListName[i] << " , --"; + } + } + + std::string portName; + if (!YarpHelper::getStringFromSearchable(rf, "wholeBodyJointsPort", portName)) + { + yError() << "[XsensRetargeting::configure] Unable to get a string from a searchable"; + return false; + } + if (!m_wholeBodyHumanJointsPort.open("/" + getName() + portName)) + { + yError() << "[XsensRetargeting::configure] " << portName << " port already open."; + return false; + } + + if (!YarpHelper::getStringFromSearchable(rf, "controllerPort", portName)) + { + yError() << "[XsensRetargeting::configure] Unable to get a string from a searchable"; + return false; + } + if (!m_wholeBodyHumanSmoothedJointsPort.open("/" + getName() + portName)) + { + yError() << "[XsensRetargeting::configure] Unable to open the port " << portName; + return false; + } + + // I should do a maping between two vectors here. + + bool foundMatch = false; + for (unsigned i = 0; i < m_robotJointsListNames.size(); i++) + { + for (unsigned j = 0; j < m_humanJointsListName.size(); j++) + { + + if (m_robotJointsListNames[i] == m_humanJointsListName[j]) + { + foundMatch = true; + m_humanToRobotMap.push_back(j); + break; + } + } + if (!foundMatch) + { + yError() << "not found match for: " << m_robotJointsListNames[i] << " , " << i; + ; + return false; + } + foundMatch = false; + } + + yInfo() << "*** mapped joint names: ****"; + for (size_t i = 0; i < m_robotJointsListNames.size(); i++) + { + yInfo() << "(" << i << ", " << m_humanToRobotMap[i] << "): " << m_robotJointsListNames[i] + << " , " << m_humanJointsListName[(m_humanToRobotMap[i])]; + } + + m_firstIteration = true; + + double jointThreshold; + if (!YarpHelper::getDoubleFromSearchable(rf, "jointDifferenceThreshold", jointThreshold)) + { + yError() << "[XsensRetargeting::configure] Unable to find the whole body joint difference " + "threshold."; + return false; + } + m_jointDiffThreshold = jointThreshold; + + yInfo() << "[XsensRetargeting::configure]" + << " Sampling time : " << m_dT; + yInfo() << "[XsensRetargeting::configure]" + << " Smoothing time : " << smoothingTime; + yInfo() << "[XsensRetargeting::configure]" + << " Joint threshold: " << m_jointDiffThreshold; + + yInfo() << " [XsensRetargeting::configure] done!"; + return true; +} +bool XsensRetargeting::getJointValues() +{ + yarp::os::Bottle* desiredHumanJoints = m_wholeBodyHumanJointsPort.read(false); + + if (desiredHumanJoints == NULL) + { + return true; + } + + // in the msg thrift the first list is the joint names [get(0)], second the joint values + // [get(1)] + yarp::os::Value humanjointsValueS = desiredHumanJoints->get(1); + yarp::os::Bottle* tmpHumanNewJointValues = humanjointsValueS.asList(); + + yarp::sig::Vector newJointValues; + newJointValues.resize(m_actuatedDOFs, 0.0); + + if (!m_firstIteration) + { + for (unsigned j = 0; j < m_actuatedDOFs; j++) + { + newJointValues(j) = tmpHumanNewJointValues->get(m_humanToRobotMap[j]).asDouble(); + // check for the spikes in joint values + if (std::abs(newJointValues(j) - m_jointValues(j)) < m_jointDiffThreshold) + { + m_jointValues(j) = newJointValues(j); + } else + { + yWarning() << "spike in data: joint : " << j << " , " << m_robotJointsListNames[j] + << " ; old data: " << m_jointValues(j) + << " ; new data:" << newJointValues(j); + } + } + } else + { + yInfo() << "[XsensRetargeting::getJointValues] Xsens Retargeting Module is Running ..."; + m_firstIteration = false; + for (unsigned j = 0; j < m_actuatedDOFs; j++) + { + newJointValues(j) = tmpHumanNewJointValues->get(m_humanToRobotMap[j]).asDouble(); + m_jointValues(j) = newJointValues(j); + } + m_WBTrajectorySmoother->init(m_jointValues); + } + + yInfo() << "joint [0]: " << m_robotJointsListNames[0] << " : " << newJointValues(0) << " , " + << m_jointValues(0); + + return true; +} + +bool XsensRetargeting::getSmoothedJointValues(yarp::sig::Vector& smoothedJointValues) +{ + m_WBTrajectorySmoother->computeNextValues(m_jointValues); + smoothedJointValues = m_WBTrajectorySmoother->getPos(); + + return true; +} + +double XsensRetargeting::getPeriod() +{ + return m_dT; +} + +bool XsensRetargeting::updateModule() +{ + + getJointValues(); + + if (m_wholeBodyHumanJointsPort.isClosed()) + { + yError() << "[XsensRetargeting::updateModule] m_wholeBodyHumanJointsPort port is closed"; + return false; + } + + if (!m_firstIteration) + { + + yarp::sig::Vector& refValues = m_wholeBodyHumanSmoothedJointsPort.prepare(); + getSmoothedJointValues(refValues); + m_wholeBodyHumanSmoothedJointsPort.write(); + } + + return true; +} + +bool XsensRetargeting::close() +{ + return true; +} diff --git a/modules/Xsens_module/src/main.cpp b/modules/Xsens_module/src/main.cpp new file mode 100644 index 00000000..5be118a3 --- /dev/null +++ b/modules/Xsens_module/src/main.cpp @@ -0,0 +1,36 @@ +/** + * @file main.cpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +// YARP +#include +#include +#include +#include + +int main(int argc, char* argv[]) +{ + // initialise yarp network + yarp::os::Network yarp; + if (!yarp.checkNetwork()) + { + yError() << "[main] Unable to find YARP network"; + return EXIT_FAILURE; + } + + // prepare and configure the resource finder + yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + + rf.setDefaultConfigFile("XsensRetargeting.ini"); + + rf.configure(argc, argv); + + // create the module + XsensRetargeting module; + + return module.runModule(rf); +}