Merge pull request #89 from robotology/demoErzelli

Add iCub Erzelli iCub2.7 robot application files to the walking teleoperation
robotology · Feb 3, 2022 · 63dfbde · 63dfbde
2 parents 2fcd1bd + a2e42e5
commit 63dfbde
Show file tree

Hide file tree

Showing 17 changed files with 649 additions and 0 deletions.
diff --git a/app/robots/iCubErzelli02/XsensRetargetingWalking.ini b/app/robots/iCubErzelli02/XsensRetargetingWalking.ini
@@ -0,0 +1,18 @@
+name                    XsensRetargeting
+samplingTime            0.01
+robot                   icub
+# if the following is enabled we will use smoothing with smoothingTime value (default is true)
+useSmoothing 0
+smoothingTime   0.25
+# The max difference (threshold) of a joint value coming from the human (rad)
+jointDifferenceThreshold 0.5
+wholeBodyJointsPort           /HumanStateWrapper/state:i
+controllerJointsPort     /jointPosition:o
+controllerCoMPort     /CoM:o
+
+# ROBOT JOINT LIST (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")
diff --git a/app/robots/iCubErzelli02/XsensRetargetingYoga.ini b/app/robots/iCubErzelli02/XsensRetargetingYoga.ini
@@ -0,0 +1,19 @@
+name                    XsensRetargeting
+samplingTime            0.01
+robot                   icub
+# if the following is enabled we will use smoothing with smoothingTime value (default is true)
+useSmoothing 1
+smoothingTime   0.25
+# The max difference (threshold) of a joint value coming from the human (rad)
+jointDifferenceThreshold 0.5
+wholeBodyJointsPort           /HumanStateWrapper/state:i
+controllerJointsPort     /jointPosition:o
+controllerCoMPort     /CoM:o
+
+# ROBOT JOINT LIST (Notice the order of the joint list is not wrong)
+# Indeed they are written according to the joint order of the yoga-retargeting
+joints_list (            "torso_pitch", "torso_roll", "torso_yaw",
+                         "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow",
+                         "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow",
+                         "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll",
+                         "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")
diff --git a/app/robots/iCubErzelli02/faceExpressionsConfig.ini b/app/robots/iCubErzelli02/faceExpressionsConfig.ini
@@ -0,0 +1,14 @@
+# The name of the module
+name                          face_expressions_from_mic
+
+# The audio input port. Connect to this port the source to be used to trigger the face expressions.
+audio_input_port_name         /in
+
+# The emotions output port. Connect this port to the emotion interface input port.
+emotions_output_port_name     /emotions:o
+
+# The module period. In any case, it depends on the frequency of the input port.
+period                        0.01
+
+# It controls the frequency with which the robot opens and close the mouth.
+expression_counter_max_value  2
diff --git a/app/robots/iCubErzelli02/hapticGloveConfig.ini b/app/robots/iCubErzelli02/hapticGloveConfig.ini
@@ -0,0 +1,25 @@
+name                    HapticGloveRetargeting
+
+
+[GENERAL]
+robot                   icub
+samplingTime            0.01
+enableMoveRobot         1
+enableLogger            1
+useLeftHand             1
+useRightHand            1
+smoothingTime           0.25
+isMandatory             0
+#calibrationTimePeriod [sec]
+calibrationTimePeriod   2.0
+#robotInitializationTime [sec]
+robotInitializationTime 10.0
+getHumanMotionRange     1
+# the counter to check if steady state is reached [steps]
+steadyStateCounterThreshold 5
+# the threshold to check the error between the desired and feedback values for the steady state check [rad]
+steadyStateThreshold        0.05
+
+# include fingers parameters
+[include LEFT_FINGERS_RETARGETING   "leftFingersHapticRetargetingParams.ini"]
+[include RIGHT_FINGERS_RETARGETING "rightFingersHapticRetargetingParams.ini"]
diff --git a/app/robots/iCubErzelli02/headRetargetingParams.ini b/app/robots/iCubErzelli02/headRetargetingParams.ini
@@ -0,0 +1,8 @@
+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
+PreparationSmoothingTime 3.0
+PreparationJointReferenceValues (0.0 , 0.0 , 0.0)
diff --git a/app/robots/iCubErzelli02/iFeelRetargetingWalking.ini b/app/robots/iCubErzelli02/iFeelRetargetingWalking.ini
@@ -0,0 +1,17 @@
+name                    iFeelRetargeting
+samplingTime            0.01
+robot                   icub
+# if the following is enabled we will use smoothing with smoothingTime value (default is true)
+useSmoothing 0
+smoothingTime   0.25
+# The max difference (threshold) of a joint value coming from the human (rad)
+jointDifferenceThreshold 0.5
+wholeBodyJointsPort           /HumanStateWrapper/state:i
+controllerJointsPort     /jointPosition:o
+controllerCoMPort     /CoM:o
+
+# ROBOT JOINT LIST (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 (            "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")
diff --git a/app/robots/iCubErzelli02/leftFingersHapticRetargetingParams.ini b/app/robots/iCubErzelli02/leftFingersHapticRetargetingParams.ini
@@ -0,0 +1,158 @@
+###############
+### ROBOT
+###############
+remote_control_boards   ("left_arm")
+
+remote_sensor_boards    "left_hand"
+
+axis_list             ( "l_thumb_oppose", "l_thumb_proximal", "l_thumb_distal", "l_index_proximal", "l_index_distal", "l_middle_proximal", "l_middle_distal", "l_pinky" )
+
+all_axis_list         ( "l_hand_finger", "l_thumb_oppose", "l_thumb_proximal", "l_thumb_distal", "l_index_proximal", "l_index_distal", "l_middle_proximal", "l_middle_distal", "l_pinky" )
+
+joint_list                   (  "l_thumb_oppose", "l_thumb_proximal", "l_thumb_middle", "l_thumb_distal",
+                                "l_index_abduction", "l_index_proximal", "l_index_middle", "l_index_distal",
+                                "l_middle_proximal", "l_middle_middle", "l_middle_distal",
+                                "l_ring_abduction", "l_ring_proximal",   "l_ring_middle",   "l_ring_distal",
+                                "l_pinky_abduction", "l_pinky_proximal",   "l_pinky_middle",   "l_pinky_distal" )
+
+# if a joint is not listed here, the sensory information is of encoder type.
+# notice that "l_thumb_oppose" and "l_hand_fingers" is measured by joint encoders not the analog sensors
+analog_list           (  "l_thumb_proximal", "l_thumb_middle", "l_thumb_distal",
+                         "l_index_proximal", "l_index_middle", "l_index_distal",
+                         "l_middle_proximal", "l_middle_middle", "l_middle_distal",
+                         "l_ring_proximal",   "l_ring_middle",   "l_ring_distal",
+                         "l_pinky_proximal",   "l_pinky_middle",   "l_pinky_distal" )
+
+#Adding the groups of each axis and associated joints
+l_hand_finger          ( "l_index_abduction", "l_ring_abduction", "l_pinky_abduction" )
+l_thumb_oppose         ( "l_thumb_oppose" )
+l_thumb_proximal       ( "l_thumb_proximal" )
+l_thumb_distal         ( "l_thumb_middle", "l_thumb_distal"  )
+l_index_proximal       ( "l_index_proximal" )
+l_index_distal         (  "l_index_middle", "l_index_distal" )
+l_middle_proximal      (  "l_middle_proximal" )
+l_middle_distal        ( "l_middle_middle", "l_middle_distal" )
+l_pinky                ( "l_ring_proximal",   "l_ring_middle",   "l_ring_distal", "l_pinky_proximal",   "l_pinky_middle",   "l_pinky_distal" )
+
+
+###############
+### HUMAN
+###############
+human_joint_list      ( "l_thumb_oppose",     "l_thumb_proximal",   "l_thumb_middle",  "l_thumb_distal",
+                        "l_index_abduction",  "l_index_proximal",   "l_index_middle",  "l_index_distal",
+                        "l_middle_abduction", "l_middle_proximal",  "l_middle_middle", "l_middle_distal",
+                        "l_ring_abduction",   "l_ring_proximal",    "l_ring_middle",   "l_ring_distal",
+                        "l_pinky_abduction",  "l_pinky_proximal",   "l_pinky_middle",  "l_pinky_distal" )
+
+human_finger_list ( "l_thumb_finger", "l_index_finger", "l_middle_finger", "l_ring_finger", "l_little_finger")
+
+hand_link           l_hand
+
+wearable_data_ports ("/WearableData/HapticGlove/LeftHand/data:o")
+
+use_rpc               0
+
+wearable_rpc_ports ( "/WearableData/HapticGlove/LeftHand/metadataRpc:o" )
+
+wearable_data_actuator_ports_out "/WearableData/HapticGlove/LeftHand/Actuators/input:o"
+
+wearable_data_actuator_ports_in "/WearableData/HapticGlove/LeftHand/Actuators/input:i"
+
+###############
+### RETARGETING
+###############
+
+motorsJointsCoupled      1
+
+# haptic feedback retargeting from the robot axis groups to the human finger
+l_thumb_finger                  ( "l_thumb_oppose", "l_thumb_proximal", "l_thumb_distal" )
+l_index_finger                  ( "l_hand_finger", "l_index_proximal", "l_index_distal" )
+l_middle_finger                 ( "l_middle_proximal", "l_middle_distal"  )
+l_ring_finger                   (   "l_pinky" )
+l_little_finger                 (   "l_pinky" )
+
+# This gain is multiplied to the total error for each motor/axis to compute the force feedback to the user associated with each axis,
+# defined according to user experience
+# Number of gains = number of motors/axis
+gainTotalError            ( 0.0 0.0 600.0     600.0 600.0 600.0      600.0 600.0 600.0 )
+
+# check this issue for the velocity Gain: https://github.com/dic-iit/element_retargeting-from-human/issues/141
+# number of gains = number of motors/axis
+gainVelocityError         ( 0.0 0.0 0.1       0.1 0.1 0.1            0.1 0.1 0.1 )
+
+# this value is multiplied to forcefeedback and provides haptic feedback to the user
+gainVibrotactile          ( 0.8 0.8 0.8 0.8 0.8 )
+
+# scaling and biased values for maping the human to robot motion
+# the sign of the following vector is important, while the absolute values will be found at configuration file
+human_to_robot_joint_angles_scaling (  -1.0 1.0 1.0 1.0
+                                        1.0 1.0 1.0 1.0
+                                        1.0 1.0 1.0
+                                        -1.0 1.0 1.0 1.0
+                                        -1.0 1.0 1.0 1.0 )
+
+human_to_robot_joint_anlges_bias    (  0.0 0.0 0.0 0.0
+                                       0.0 0.0 0.0 0.0
+                                       0.0 0.0 0.0
+                                       0.0 0.0 0.0 0.0
+                                       0.0 0.0 0.0 0.0  )
+
+axisContactThreshold                 0.1
+
+##############################
+### ROBOT CONTROL & ESTIMATION
+##############################
+useVelocity           0
+
+referenceVelocityForPositionControl 100.0
+
+# minimum and maximum values of the joints
+# related to analog sensors
+analog_joints_min_boundary ( 0.0 0.0 0.0     0.0 0.0 0.0     0.0 0.0 0.0     0.0 0.0 0.0     0.0 0.0 0.0  )
+analog_joints_max_boundary ( 90.0 90.0 90.0  90.0 90.0 90.0  90.0 90.0 90.0  90.0 90.0 90.0  90.0 90.0 90.0 )
+
+# hall sensors minimum and maximum values
+analog_sensors_raw_min_boundary ( 249.0 221.0 255.0   255.0 240.0 253.0    248.0 245.0 244.0     255.0 247.0 255.0    245.0 248.0 247.0  )
+analog_sensors_raw_max_boundary ( 2.0 29.0 7.0         0.0 4.0 0.0        8.0 54.0 0.0         36.0 0.0 0.0       0.0 15.0 0.0  )
+
+
+# in case each joint does not have independant motor to actuate, they are coupled
+axesJointsCoupled     1
+
+# in case a calibration phase necessary before starting teleoperating the robot,
+# otherwise read the calibration matrix from the config file
+doCalibration           1
+
+#robot controller exponential filter gain
+exponentialFilterGain           0.9
+
+# q= A x m, where:
+#       q (n,1) is the joint values
+#       m (m,1) is the motors values
+#       A (n,m) is the mapping from the motors values to the joint values
+# CouplingMatrix = A : (n,m) matrix
+CouplingMatrix          ( 1.1 0.0 0.0 0.0 0.0
+                          0.0 1.0 1.0 0.0 0.0
+                          0.0 0.0 1.0 0.0 0.0
+                          0.0 0.0 0.0 0.0 1.0 )
+
+# in the Quadratic optimizartion problem to compute the motor values from the joint values : xT Q X + uT R u
+# q_matrix_joint_motor is a list that identifies the main diagonal values of matrix Q: (q x q) matrix;
+# size of q: is the number of desired joints to control
+# r_matrix_joint_motor is a list that identifies the main diagonal of matrix R: (m x m) matrix;
+# size of m: is the number of desired motors to control
+
+q_qp_control    ( 1.0 1.0 1.0 1.0     1.0 1.0 1.0 1.0      1.0 1.0 1.0     1.0 1.0 1.0 1.0     1.0 1.0 1.0 1.0 )
+
+r_qp_control    ( 0.0 0.0 0.0     0.0 0.0 0.0     0.0 0.0 0.0 )
+
+
+# in the Kalman filter problem to estimate the axis values, velocity and acceleration:
+# q_matrix_kf is a list that identifies the main diagonal of matrix: E[ (w(t) -w_bar(t)) (w(t) -w_bar(t))^T ],
+# size:  m*m positive matrix,  Dx(t)= Fx(t)+ Gw(t), the process noise */
+# r_matrix_kf is a list that identifies the main diagonal of matrix: E[ v(t) v(t)^T ],
+# size:  p*p positive matrix, Z(t)= Hx(t)+ v(t), the measurement noise
+no_states_kf            3
+no_measurement_kf       1
+q_matrix_kf             ( 10.0 150.0 100000.0 )
+r_matrix_kf             ( 0.0000001 )
diff --git a/app/robots/iCubErzelli02/leftFingersRetargetingParams.ini b/app/robots/iCubErzelli02/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/iCubErzelli02/leftHandRetargetingParams.ini b/app/robots/iCubErzelli02/leftHandRetargetingParams.ini
@@ -0,0 +1,17 @@
+# additional rotations
+# the following rotation map the hand oculus frame and the hand robot frame
+handOculusFrame_R_handRobotFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.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/iCubErzelli02/oculusConfig.ini b/app/robots/iCubErzelli02/oculusConfig.ini
@@ -0,0 +1,80 @@
+name                    oculusRetargeting
+
+# ports
+leftHandPosePort        /leftHandPose:o
+rightHandPosePort       /rightHandPose:o
+playerOrientationPort   /playerOrientation:i
+rpcWalkingPort_name     /walkingRpc
+rpcVirtualizerPort_name /virtualizerRpc
+rpcServerOculusPort_name /oculusRpc
+
+[GENERAL]
+samplingTime            0.01
+robot                   icub
+useXsens                0
+useiFeel                1
+useOpenXr               1
+useSenseGlove           0
+enableLogger            0
+enableMoveRobot         1
+# the following value is a threshold used to update the teleoperation frame position
+# when the human rotates inside the virtualizer
+playerOrientationThreshold    0.2
+
+# For kinematic scaling for task-space retargeting
+humanHeight                   1.76
+# The robot arm span is the distance between the left and right index fingertips when the arms are aligned, i.e. T pose.
+robotArmSpan                  1.27
+# If resetCameras !=0, the module will attempt to reset the cameras during the initialization phase.
+resetCameras            0
+# The leftCameraPort and rightCameraPort are needed only if resetCameras is 1
+leftCameraPort          /icub/cam/left
+rightCameraPort         /icub/cam/right
+
+
+# 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
+oculusPositionPort              /oculusPosition:i
+
+move_icub_using_joypad        1
+deadzone                      0.3
+fullscale                     1.0
+scale_X                       5.0
+scale_Y                       5.0
+use_left                      1
+
+
+[OPENXR]
+root_frame_name                 openxr_origin
+left_hand_frame_name            openxr_left_hand
+right_hand_frame_name           openxr_right_hand
+head_frame_name                 openxr_head
+oculusOrientationPort           /oculusOrientation:i
+oculusPositionPort              /oculusPosition: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/iCubErzelli02/openXRHeadsetParameters.ini b/app/robots/iCubErzelli02/openXRHeadsetParameters.ini
@@ -0,0 +1,19 @@
+### Device params
+device             JoypadControlServer
+use_separate_ports 1
+period             10
+name               /joypadDevice/Oculus
+subdevice          openxrheadset
+
+### Joypad params
+stick_as_axis      true
+
+### Gui params
+gui_elements       1
+
+[GUI_0]
+width              0.15
+height             0.15
+x                  0.15
+y                 -0.2
+z                 -0.5