diff --git a/app/robots/iCubGenova09/XsensRetargetingWalking.ini b/app/robots/iCubGenova09/XsensRetargetingWalking.ini new file mode 100644 index 00000000..ff4f55de --- /dev/null +++ b/app/robots/iCubGenova09/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/iCubGenova09/XsensRetargetingYoga.ini b/app/robots/iCubGenova09/XsensRetargetingYoga.ini new file mode 100644 index 00000000..d84d6067 --- /dev/null +++ b/app/robots/iCubGenova09/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/iCubGenova09/headRetargetingParams.ini b/app/robots/iCubGenova09/headRetargetingParams.ini new file mode 100644 index 00000000..a08d72eb --- /dev/null +++ b/app/robots/iCubGenova09/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/iCubGenova09/leftFingersRetargetingParams.ini b/app/robots/iCubGenova09/leftFingersRetargetingParams.ini new file mode 100644 index 00000000..9307768d --- /dev/null +++ b/app/robots/iCubGenova09/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/iCubGenova09/leftHandRetargetingParams.ini b/app/robots/iCubGenova09/leftHandRetargetingParams.ini new file mode 100644 index 00000000..1bb1db46 --- /dev/null +++ b/app/robots/iCubGenova09/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/iCubGenova09/oculusConfig.ini b/app/robots/iCubGenova09/oculusConfig.ini new file mode 100644 index 00000000..9b1351b8 --- /dev/null +++ b/app/robots/iCubGenova09/oculusConfig.ini @@ -0,0 +1,60 @@ +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 +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.0 +# If resetCameras !=0, the module will attempt to reset the cameras during the initialization phase. +resetCameras 1 +# 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 + +# root_frame_name oculusworld +# head_frame_name headoculus diff --git a/app/robots/iCubGenova09/rightFingersRetargetingParams.ini b/app/robots/iCubGenova09/rightFingersRetargetingParams.ini new file mode 100644 index 00000000..d4af0664 --- /dev/null +++ b/app/robots/iCubGenova09/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/iCubGenova09/rightHandRetargetingParams.ini b/app/robots/iCubGenova09/rightHandRetargetingParams.ini new file mode 100644 index 00000000..1bb1db46 --- /dev/null +++ b/app/robots/iCubGenova09/rightHandRetargetingParams.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/iCubGenova09/torsoRetargeting.ini b/app/robots/iCubGenova09/torsoRetargeting.ini new file mode 100644 index 00000000..7f8a6f0b --- /dev/null +++ b/app/robots/iCubGenova09/torsoRetargeting.ini @@ -0,0 +1,2 @@ +remote_control_boards ("torso") +joints_list ("torso_pitch", "torso_roll", "torso_yaw") diff --git a/app/scripts/xprizeDemoVirtualizerXsensGlove.xml b/app/scripts/xprizeDemoVirtualizerXsensGlove.xml new file mode 100644 index 00000000..5db9e687 --- /dev/null +++ b/app/scripts/xprizeDemoVirtualizerXsensGlove.xml @@ -0,0 +1,243 @@ + + + + + + + Xrprize Demo (Virtualizer Xsens SenseGlove) + Application to run the ANA Avatar XPRIZE demo. + 1.0 + + Giulio Romualdi + + + + + + + yarpdev + --from camera/ServerGrabberDualDragon.ini --split true + icub-head + + + + WalkingModule + icub-head + --from dcm_walking_joint_retargeting.ini + + + + yarpdev + --device AudioRecorderWrapper --subdevice portaudioRecorder --name /icub/microphone --min_samples_over_network 4000 --max-samples_over_network 4000 --rate 160000 --samples 16000 + icub-head + + + + yarpdev + --device AudioPlayerWrapper --subdevice portaudioPlayer --name /icub/speakers --playback_network_buffer_lenght 0.4 + icub-head + + /icub-virtualizer/microphone/rpc + + + + + + + camCalibWithPose + --context cameraCalibration --from icubEyes.ini --group CAMERA_CALIBRATION_LEFT --name /icub/camcalib/left + icub-console + + + + camCalibWithPose + --context cameraCalibration --from icubEyes.ini --group CAMERA_CALIBRATION_RIGHT --name /icub/camcalib/right + icub-console + + + + + 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 --userpose true --gui_elements 1 --GUI_0::width 0.15 --GUI_0::height 0.15 --GUI_0::x 0.2 --GUI_0::y -0.2 --GUI_0::z -0.5 --GUI_0::alpha 1.0 --hfov 59 --left-pitch-offset 0.0725 --left-yaw-offset -0.065 --right-pitch-offset 0.13 --right-yaw-offset 0.07 + icub-virtualizer + + + + VirtualizerModule + icub-virtualizer + --context virtualizerRetargeting + + + + yarpdev + --device AudioRecorderWrapper --subdevice portaudioRecorder --name /icub-virtualizer/microphone --min_samples_over_network 4000 --max-samples_over_network 4000 --rate 160000 --samples 16000 --channels 1 + icub-virtualizer + + + + yarpdev + --device AudioPlayerWrapper --subdevice portaudioPlayer --name /icub-virtualizer/speakers --playback_network_buffer_lenght 0.4 + icub-virtualizer + + /icub/microphone/rpc + + + + + + + OculusRetargetingModule + icub-console-gui + --OCULUS::move_icub_using_joypad 0 --GENERAL::useSenseGlove 1 + + /transformServer/transforms:o + /joypadDevice/Oculus/rpc:i + /icub/cam/left + /icub/cam/right + + + + + XsensRetargetingModule + icub-console-gui + + + + HapticGloveModule + icub-console-gui + + + + dyn-visualizer + icub-console-gui + + + + face-expressions-from-mic + icub-console-gui + + + + + /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 + mjpeg + + + + /icub/camcalib/right/out + /oculus/display/right:i + mjpeg + + + + /iCub/RobotStateWrapper/state:o + /XsensRetargeting/HumanStateWrapper/state:i + + + + /XsensRetargeting/jointPosition:o + /walking-coordinator/jointPosition:i + + + + /XsensRetargeting/CoM:o + /walking-coordinator/CoM:i + + + + /icub-virtualizer/microphone/audio:o + /icub/speakers/audio:i + fast_tcp + + + + /icub/microphone/audio:o + /icub-virtualizer/speakers/audio:i + fast_tcp + + + + /visualizerImage + /oculus/gui_0 + mjpeg + + + + /icub-virtualizer/microphone/audio:o + /face_expressions_from_mic/in + + + + /face_expressions_from_mic/emotions:o + /icub/face/emotions/in + + + diff --git a/app/virtualizerRetargeting/virtualizerConfig.ini b/app/virtualizerRetargeting/virtualizerConfig.ini index d29d3113..535bfa5d 100644 --- a/app/virtualizerRetargeting/virtualizerConfig.ini +++ b/app/virtualizerRetargeting/virtualizerConfig.ini @@ -8,5 +8,5 @@ deadzone 0.35 playerOrientationPort_name /playerOrientation:o robotOrientationPort_name /robotOrientation:i rpcWalkingPort_name /walkingRpc -scale_X 2.0 -scale_Y 1.0 +scale_X 3.0 +scale_Y 2.0 diff --git a/modules/Oculus_module/include/OculusModule.hpp b/modules/Oculus_module/include/OculusModule.hpp index ddcf62d1..c2353101 100644 --- a/modules/Oculus_module/include/OculusModule.hpp +++ b/modules/Oculus_module/include/OculusModule.hpp @@ -74,6 +74,7 @@ class OculusModule : public yarp::os::RFModule unsigned int m_releaseRightIndex; /**< Index of the trigger used for releasing the right hand */ unsigned int m_startWalkingIndex; /**< Index of the start walking button */ + unsigned int m_stopWalkingIndex; /**< Index of the stop walking button */ unsigned int m_prepareWalkingIndex; /**< Index of the prepare walking button */ bool m_useVirtualizer; /**< True if the virtualizer is used in the retargeting */ diff --git a/modules/Oculus_module/src/OculusModule.cpp b/modules/Oculus_module/src/OculusModule.cpp index d08d1fcf..f2a15969 100644 --- a/modules/Oculus_module/src/OculusModule.cpp +++ b/modules/Oculus_module/src/OculusModule.cpp @@ -189,6 +189,7 @@ bool OculusModule::configureJoypad(const yarp::os::Searchable& config) m_releaseRightIndex = 3; m_startWalkingIndex = 4; // X button + m_stopWalkingIndex = 5; // X button m_prepareWalkingIndex = 0; // A button return true; @@ -812,6 +813,25 @@ bool OculusModule::updateModule() } } + // check if it is time to prepare or start walking + float buttonMapping; + + // prepare robot (A button) + m_joypadControllerInterface->getButton(m_stopWalkingIndex, buttonMapping); + yarp::os::Bottle cmd, outcome; + + if (buttonMapping > 0) + { + // TODO add a visual feedback for the user + if (m_moveRobot) + { + cmd.addString("stopWalking"); + m_rpcWalkingClient.write(cmd, outcome); + } + yInfo() << "[OculusModule::updateModule] stop"; + return false; + } + #ifdef ENABLE_LOGGER if (m_enableLogger) {