Merge pull request #19 from kouroshD/devel_Xsens_rebased

robotology · Jun 4, 2019 · 04e3520 · 04e3520
2 parents a1e6948 + 5ece81c
commit 04e3520
Show file tree

Hide file tree

Showing 36 changed files with 1,731 additions and 99 deletions.
diff --git a/CMakeLists.txt.user b/CMakeLists.txt.user
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -54,6 +54,7 @@
   <module>
     <name>OculusRetargetingModule</name>
     <node>icub-head</node>
+    <parameters> --useXsens 0 </parameters>
   </module>
 
   <!-- Connections -->

diff --git a/app/scripts/dcmWalkingRetargetingVirtualizer.xml b/app/scripts/dcmWalkingRetargetingVirtualizer.xml
@@ -58,7 +58,7 @@
   <module>
     <name>OculusRetargetingModule</name>
     <node>icub-head</node>
-    <parameters> --OCULUS::move_icub_using_joypad 0 </parameters>
+    <parameters> --OCULUS::move_icub_using_joypad 0 --useXsens 0 </parameters>
   </module>
 
   <!-- Connections -->
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1,2 @@
		remote_control_boards ("torso")
		joints_list ("torso_pitch", "torso_roll", "torso_yaw")