From 9462bffbf3db86c17c4add4d7936bd27de58d834 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Mon, 8 Jul 2024 18:27:01 +0200 Subject: [PATCH 01/11] Added conf file for IK using 5 trackers For ergoCub --- ...StateProvider_ergoCub_openxr_5trackers.xml | 191 ++++++++++++++++++ 1 file changed, 191 insertions(+) create mode 100644 conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml b/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml new file mode 100644 index 00000000..a9bce97a --- /dev/null +++ b/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml @@ -0,0 +1,191 @@ + + + + + + 0.01 + ftc_yarp_only.xml + /IFrameTransformToIWear/tf + /IFrameTransformToIWear/tf/local_rpc + + + + TransformServer + root_link_desired + PoseSensor + (root_link_desired + openxr_head + vive_tracker_right_elbow_pose + vive_tracker_left_elbow_pose + vive_tracker_right_shoulder_pose + vive_tracker_left_shoulder_pose) + + + TransformClient + + + + + + + () + false + + + IFrameTransformToIWear + + + + + + 0.01 + model.urdf + root_link + + dynamical + true + false + + 300 + ma27 + 2 + 0.0 + 1.0 + 1.0 + 0.001 + ergoCub + + + QP + 1.0 + 1.0 + + 10.0 + (1.0 1.0) + (700.0 20.0) + (0.0 0.0) + + (root_link, TransformServer::pose::root_link_desired, orientation) + (r_hand_palm, TransformServer::pose::vive_tracker_right_elbow_pose, pose) + (l_hand_palm, TransformServer::pose::vive_tracker_left_elbow_pose, pose) + (head, TransformServer::pose::openxr_head, pose) + (r_upper_arm TransformServer::pose::vive_tracker_right_shoulder_pose, orientation) + (l_upper_arm TransformServer::pose::vive_tracker_left_shoulder_pose, orientation) + + ("l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "l_wrist_roll", + "l_wrist_pitch", + "l_wrist_yaw", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "r_wrist_roll", + "r_wrist_pitch", + "r_wrist_yaw", + "torso_pitch", + "torso_roll", + "torso_yaw", + "neck_pitch", + "neck_roll", + "neck_yaw") + + + ( 1.0 0.0 0.0 0.0 + 0.0 0.0 1.0 -0.02 + 0.0 -1.0 0.0 0.05 + 0.0 0.0 0.0 1.0) + (-1.0 0.0 0.0 0.0 + 0.0 0.0 1.0 -0.02 + 0.0 1.0 0.0 0.05 + 0.0 0.0 0.0 1.0) + ( 0.0 -1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + -1.0 0.0 0.0 0.3 + 0.0 0.0 0.0 1.0) + ( 1.0 0.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 0.0 -1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + (-1.0 0.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + + + ( 0.7 0.7 0.6 ) + ( 0.7 0.7 0.6 ) + ( 0.7 0.7 0.6 ) + 0.7 + 0.7 + 0.7 + + + + + + + ( ) + + + ( ) + + (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + + ( + l_shoulder_roll, r_shoulder_roll) + ( + (1.0, 0.0), + (0.0, 1.0)) + ( + -100.0, -100.0) + ( + 1.4, 1.4) + 0.5 + 0.5 + + + + XSenseIWearRemapper + + + + + + + + 0.01 + /ergoCub/RobotStateServer/state:o + + + RobotStateProvider + + + + + + + 0.01 + /HDE/WearableTargetsServer/state:o + + + RobotStateProvider + + + + + + From bd4ad8f31c1d9295efabcfad4b0324d4dd8e5fb5 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Mon, 8 Jul 2024 18:30:42 +0200 Subject: [PATCH 02/11] Added conf file for IK for using Ultimate Trackers and joysticks --- ...goCub_openxr_joystick_ultimateTrackers.xml | 192 ++++++++++++++++++ 1 file changed, 192 insertions(+) create mode 100644 conf/xml/RobotStateProvider_ergoCub_openxr_joystick_ultimateTrackers.xml diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr_joystick_ultimateTrackers.xml b/conf/xml/RobotStateProvider_ergoCub_openxr_joystick_ultimateTrackers.xml new file mode 100644 index 00000000..56766648 --- /dev/null +++ b/conf/xml/RobotStateProvider_ergoCub_openxr_joystick_ultimateTrackers.xml @@ -0,0 +1,192 @@ + + + + + + 0.01 + ftc_yarp_only.xml + /IFrameTransformToIWear/tf + /IFrameTransformToIWear/tf/local_rpc + 1.0 + + + + TransformServer + world_ut + PoseSensor + (world_ut + openxr_head + openxr_right_hand + openxr_left_hand + vive_tracker_right_elbow_pose + vive_tracker_left_elbow_pose) + + + TransformClient + + + + + + + () + false + + + IFrameTransformToIWear + + + + + + 0.01 + model.urdf + root_link + + dynamical + true + false + + 300 + ma27 + 2 + 0.0 + 1.0 + 1.0 + 0.001 + ergoCub + + + QP + 1.0 + 1.0 + + 10.0 + (1.0 1.0) + (700.0 20.0) + (0.0 0.0) + + (root_link, TransformServer::pose::world_ut, orientation) + (r_hand_palm, TransformServer::pose::openxr_right_hand, pose) + (l_hand_palm, TransformServer::pose::openxr_left_hand, pose) + (head, TransformServer::pose::openxr_head, pose) + (r_upperarm TransformServer::pose::vive_tracker_right_elbow_pose, orientation) + (l_upperarm TransformServer::pose::vive_tracker_left_elbow_pose, orientation) + + ("l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "l_wrist_roll", + "l_wrist_pitch", + "l_wrist_yaw", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "r_wrist_roll", + "r_wrist_pitch", + "r_wrist_yaw", + "torso_pitch", + "torso_roll", + "torso_yaw", + "neck_pitch", + "neck_roll", + "neck_yaw") + + + ( 0.0 -1.0 0.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 -1.0 0.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 -1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + -1.0 0.0 0.0 0.3 + 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 -1.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 -1.0 0.0 + 0.0 0.0 0.0 1.0) + + + ( 0.7 0.7 0.6 ) + ( 0.7 0.7 0.6 ) + ( 0.7 0.7 0.6 ) + 0.7 + 0.7 + 0.7 + + + + + + + ( ) + + + ( ) + + (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + + ( + l_shoulder_roll, r_shoulder_roll) + ( + (1.0, 0.0), + (0.0, 1.0)) + ( + -100.0, -100.0) + ( + 1.4, 1.4) + 0.5 + 0.5 + + + + XSenseIWearRemapper + + + + + + + + 0.01 + /ergoCub/RobotStateServer/state:o + + + RobotStateProvider + + + + + + + 0.01 + /HDE/WearableTargetsServer/state:o + + + RobotStateProvider + + + + + + From 6e54d19c071174583006723ec22f52cc91178f59 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Thu, 1 Aug 2024 17:26:25 +0200 Subject: [PATCH 03/11] Added application to use 5 ultimate trackers --- ...vider_ergoCub_openxr_5ultimateTrackers.xml | 191 ++++++++++++++++++ ...goCub_openxr_joystick_ultimateTrackers.xml | 2 +- 2 files changed, 192 insertions(+), 1 deletion(-) create mode 100644 conf/xml/RobotStateProvider_ergoCub_openxr_5ultimateTrackers.xml diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr_5ultimateTrackers.xml b/conf/xml/RobotStateProvider_ergoCub_openxr_5ultimateTrackers.xml new file mode 100644 index 00000000..755ae1c3 --- /dev/null +++ b/conf/xml/RobotStateProvider_ergoCub_openxr_5ultimateTrackers.xml @@ -0,0 +1,191 @@ + + + + + + 0.01 + ftc_yarp_only.xml + /IFrameTransformToIWear/tf + /IFrameTransformToIWear/tf/local_rpc + + + + TransformServer + world_ut + PoseSensor + (world_ut + openxr_head + vive_tracker_right_elbow_pose + vive_tracker_left_elbow_pose + vive_tracker_right_shoulder_pose + vive_tracker_left_shoulder_pose) + + + TransformClient + + + + + + + () + false + + + IFrameTransformToIWear + + + + + + 0.01 + model.urdf + root_link + + dynamical + true + false + + 300 + ma27 + 2 + 0.0 + 1.0 + 1.0 + 0.001 + ergoCub + + + QP + 1.0 + 1.0 + + 10.0 + (1.0 1.0) + (500.0 20.0) + (0.0 0.0) + + (root_link, TransformServer::pose::world_ut, orientation) + (r_hand_palm, TransformServer::pose::vive_tracker_right_elbow_pose, pose) + (l_hand_palm, TransformServer::pose::vive_tracker_left_elbow_pose, pose) + (head, TransformServer::pose::openxr_head, pose) + (r_upper_arm TransformServer::pose::vive_tracker_right_shoulder_pose, orientation) + (l_upper_arm TransformServer::pose::vive_tracker_left_shoulder_pose, orientation) + + ("l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "l_wrist_roll", + "l_wrist_pitch", + "l_wrist_yaw", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "r_wrist_roll", + "r_wrist_pitch", + "r_wrist_yaw", + "torso_pitch", + "torso_roll", + "torso_yaw", + "neck_pitch", + "neck_roll", + "neck_yaw") + + + (-1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 -1.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 -1.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 -1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + -1.0 0.0 0.0 0.3 + 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 -1.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 -1.0 0.0 + 0.0 0.0 0.0 1.0) + + + ( 0.7 0.7 0.6 ) + ( 0.7 0.7 0.6 ) + ( 0.7 0.7 0.6 ) + 0.7 + 0.7 + 0.7 + + + + + + + ( ) + + + ( ) + + (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + + ( + l_shoulder_roll, r_shoulder_roll) + ( + (1.0, 0.0), + (0.0, 1.0)) + ( + -100.0, -100.0) + ( + 1.4, 1.4) + 0.5 + 0.5 + + + + XSenseIWearRemapper + + + + + + + + 0.01 + /ergoCub/RobotStateServer/state:o + + + RobotStateProvider + + + + + + + 0.01 + /HDE/WearableTargetsServer/state:o + + + RobotStateProvider + + + + + + diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr_joystick_ultimateTrackers.xml b/conf/xml/RobotStateProvider_ergoCub_openxr_joystick_ultimateTrackers.xml index 56766648..adc2e4ca 100644 --- a/conf/xml/RobotStateProvider_ergoCub_openxr_joystick_ultimateTrackers.xml +++ b/conf/xml/RobotStateProvider_ergoCub_openxr_joystick_ultimateTrackers.xml @@ -71,7 +71,7 @@ 10.0 (1.0 1.0) - (700.0 20.0) + (500.0 20.0) (0.0 0.0) (root_link, TransformServer::pose::world_ut, orientation) From d1e4fb3008d60508b570dbcc9fc9e449cd1e5d06 Mon Sep 17 00:00:00 2001 From: ergocub Date: Fri, 6 Sep 2024 16:40:42 +0200 Subject: [PATCH 04/11] Updates on the 5 tracker confs before demo --- ...obotStateProvider_ergoCub_openxr_5trackers.xml | 15 +++++++++------ ...eProvider_ergoCub_openxr_5ultimateTrackers.xml | 15 +++++++++------ 2 files changed, 18 insertions(+), 12 deletions(-) diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml b/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml index a9bce97a..472c0a15 100644 --- a/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml +++ b/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml @@ -7,6 +7,7 @@ ftc_yarp_only.xml /IFrameTransformToIWear/tf /IFrameTransformToIWear/tf/local_rpc + 1.0 @@ -99,7 +100,8 @@ "torso_yaw", "neck_pitch", "neck_roll", - "neck_yaw") + "neck_yaw", + "camera_tilt") ( 1.0 0.0 0.0 0.0 @@ -146,14 +148,15 @@ ( - l_shoulder_roll, r_shoulder_roll) + l_shoulder_roll, r_shoulder_roll, camera_tilt) ( - (1.0, 0.0), - (0.0, 1.0)) + (1.0, 0.0, 0.0), + (0.0, 1.0, 0.0), + (0.0, 0.0, 1.0)) ( - -100.0, -100.0) + -100.0, -100.0, -0.1) ( - 1.4, 1.4) + 1.4, 1.4, 0.1) 0.5 0.5 diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr_5ultimateTrackers.xml b/conf/xml/RobotStateProvider_ergoCub_openxr_5ultimateTrackers.xml index 755ae1c3..d78de9f1 100644 --- a/conf/xml/RobotStateProvider_ergoCub_openxr_5ultimateTrackers.xml +++ b/conf/xml/RobotStateProvider_ergoCub_openxr_5ultimateTrackers.xml @@ -7,6 +7,7 @@ ftc_yarp_only.xml /IFrameTransformToIWear/tf /IFrameTransformToIWear/tf/local_rpc + 1.0 @@ -99,7 +100,8 @@ "torso_yaw", "neck_pitch", "neck_roll", - "neck_yaw") + "neck_yaw", + "camera_tilt") (-1.0 0.0 0.0 0.0 @@ -146,14 +148,15 @@ ( - l_shoulder_roll, r_shoulder_roll) + l_shoulder_roll, r_shoulder_roll, camera_tilt) ( - (1.0, 0.0), - (0.0, 1.0)) + (1.0, 0.0, 0.0), + (0.0, 1.0, 0.0), + (0.0, 0.0, 1.0)) ( - -100.0, -100.0) + -100.0, -100.0, -0.1) ( - 1.4, 1.4) + 1.4, 1.4, 0.1) 0.5 0.5 From ba2886e5c2ec2969946a65d6aa92fd8f3878d4b6 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Mon, 16 Sep 2024 18:56:58 +0200 Subject: [PATCH 05/11] Added possibility to move height of the base THis would allow controlling the height of the CoM on the robot --- conf/xml/RobotStateProvider_ergoCub_openxr_ifeel.xml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr_ifeel.xml b/conf/xml/RobotStateProvider_ergoCub_openxr_ifeel.xml index 507a761d..91583cf9 100644 --- a/conf/xml/RobotStateProvider_ergoCub_openxr_ifeel.xml +++ b/conf/xml/RobotStateProvider_ergoCub_openxr_ifeel.xml @@ -7,12 +7,12 @@ ftc_yarp_only.xml /IFrameTransformToIWear/tf /IFrameTransformToIWear/tf/local_rpc - 1.0 + 10.0 TransformServer - root_link_desired + world_grounded PoseSensor (root_link_desired openxr_head @@ -68,10 +68,10 @@ 10.0 (1.0 1.0) - (700.0 20.0) + (200.0 20.0) (0.0 0.0) - (root_link, TransformServer::pose::root_link_desired, orientation) + (root_link, TransformServer::pose::root_link_desired, pose) (chest, iFeelSuit::vLink::Node#6, gravity) (r_hand_palm, TransformServer::pose::vive_tracker_right_elbow_pose, pose) (l_hand_palm, TransformServer::pose::vive_tracker_left_elbow_pose, pose) @@ -155,8 +155,8 @@ () - (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.4, 0.0, 0.0, 0.0) + (0.0, 0.0, -0.4, 0.0, 0.0, 0.0) ( From ea7aa83739abfe7bcae95e6ee04cdfb702817acd Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Mon, 23 Sep 2024 13:01:14 +0200 Subject: [PATCH 06/11] Prepared configuration files to run teleoperation on iRonCub --- .../HumanStateVisualizer_iCub2_5_openxr.ini | 10 +- ...anStateVisualizer_iCub2_5_openxr_ifeel.ini | 8 +- .../HumanStateVisualizer_iRonCub_openxr.ini | 26 +++ ...anStateVisualizer_iRonCub_openxr_ifeel.ini | 26 +++ .../xml/RobotStateProvider_iCub2_5_openxr.xml | 85 +++++--- ...obotStateProvider_iCub2_5_openxr_ifeel.xml | 110 ++++++---- .../xml/RobotStateProvider_iRonCub_openxr.xml | 184 ++++++++++++++++ ...obotStateProvider_iRonCub_openxr_ifeel.xml | 198 ++++++++++++++++++ 8 files changed, 571 insertions(+), 76 deletions(-) create mode 100644 conf/app/HumanStateVisualizer_iRonCub_openxr.ini create mode 100644 conf/app/HumanStateVisualizer_iRonCub_openxr_ifeel.ini create mode 100644 conf/xml/RobotStateProvider_iRonCub_openxr.xml create mode 100644 conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml diff --git a/conf/app/HumanStateVisualizer_iCub2_5_openxr.ini b/conf/app/HumanStateVisualizer_iCub2_5_openxr.ini index 053073b0..2277b0ff 100644 --- a/conf/app/HumanStateVisualizer_iCub2_5_openxr.ini +++ b/conf/app/HumanStateVisualizer_iCub2_5_openxr.ini @@ -1,7 +1,7 @@ name HumanStateVisualizer # Model Configuration options -modelURDFName "teleoperation_iCub_model_V_2_5.urdf" +modelURDFName "model.urdf" ignoreMissingLinks true visualizeWrenches false visualizeFrames true @@ -14,15 +14,13 @@ fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera maxVisualizationFPS 65 # Link visualization option -# visualizedLinksFrame (r_hand_fake l_hand_fake root_link_fake l_upper_arm_fake l_forearm_fake r_upper_arm_fake r_forearm_fake head) -visualizedLinksFrame (r_hand_fake l_hand_fake ) +visualizedLinksFrame (head, l_hand r_hand root_link ) linksFrameScalingFactor 0.1 # Targets visualization option -visualizedTargetsFrame ( target_RightHand target_LeftHand) -# visualizedTargetsFrame ( target_RightHand target_LeftHand target_Pelvis target_l_upper_arm_fake target_l_forearm_fake target_r_upper_arm_fake target_r_forearm_fake target_Head) +visualizedTargetsFrame (target_Head target_RightHand target_LeftHand) targetsFrameScalingFactor 0.2 # Client Configuration humanStateDataPortName "/iCub/RobotStateServer/state:o" -wearableTargetsServerPortName "/HDE/WearableTargetsServer/state:o" +wearableTargetsServerPortName "/iCub/HDE/WearableTargetsServer/state:o" diff --git a/conf/app/HumanStateVisualizer_iCub2_5_openxr_ifeel.ini b/conf/app/HumanStateVisualizer_iCub2_5_openxr_ifeel.ini index 4fa7c336..0735daba 100644 --- a/conf/app/HumanStateVisualizer_iCub2_5_openxr_ifeel.ini +++ b/conf/app/HumanStateVisualizer_iCub2_5_openxr_ifeel.ini @@ -1,7 +1,7 @@ name HumanStateVisualizer # Model Configuration options -modelURDFName "teleoperation_iCub_model_V_2_5.urdf" +modelURDFName "model.urdf" ignoreMissingLinks true visualizeWrenches false visualizeFrames true @@ -14,13 +14,13 @@ fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera maxVisualizationFPS 65 # Link visualization option -visualizedLinksFrame (r_hand_fake l_hand_fake root_link_fake l_upper_arm_fake l_forearm_fake r_upper_arm_fake r_forearm_fake head) +visualizedLinksFrame (root_link l_shoulder_3 l_forearm r_shoulder_3 r_forearm head chest) linksFrameScalingFactor 0.1 # Targets visualization option -visualizedTargetsFrame ( target_RightHand target_LeftHand target_Pelvis target_l_upper_arm_fake target_l_forearm_fake target_r_upper_arm_fake target_r_forearm_fake target_Head) +visualizedTargetsFrame ( target_RightHand target_LeftHand target_Pelvis target_Head target_Chest target_r_upper_arm target_l_upper_arm target_r_forearm target_l_forearm) targetsFrameScalingFactor 0.2 # Client Configuration humanStateDataPortName "/iCub/RobotStateServer/state:o" -wearableTargetsServerPortName "/HDE/WearableTargetsServer/state:o" +wearableTargetsServerPortName "/iCub/HDE/WearableTargetsServer/state:o" diff --git a/conf/app/HumanStateVisualizer_iRonCub_openxr.ini b/conf/app/HumanStateVisualizer_iRonCub_openxr.ini new file mode 100644 index 00000000..5d0ec173 --- /dev/null +++ b/conf/app/HumanStateVisualizer_iRonCub_openxr.ini @@ -0,0 +1,26 @@ +name HumanStateVisualizer + +# Model Configuration options +modelURDFName "model.urdf" +ignoreMissingLinks true +visualizeWrenches false +visualizeFrames true +visualizeTargets true + +# Camera options +cameraDeltaPosition (0.0, 2.0, 0.5) +useFixedCamera true # if set to false, the camera follows the model base link +fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false +maxVisualizationFPS 65 + +# Link visualization option +visualizedLinksFrame (head, r_elbow_1 l_elbow_1 root_link ) +linksFrameScalingFactor 0.1 + +# Targets visualization option +visualizedTargetsFrame (target_Head target_RightHand target_LeftHand) +targetsFrameScalingFactor 0.2 + +# Client Configuration +humanStateDataPortName "/iCub/RobotStateServer/state:o" +wearableTargetsServerPortName "/iCub/HDE/WearableTargetsServer/state:o" diff --git a/conf/app/HumanStateVisualizer_iRonCub_openxr_ifeel.ini b/conf/app/HumanStateVisualizer_iRonCub_openxr_ifeel.ini new file mode 100644 index 00000000..7bf9f200 --- /dev/null +++ b/conf/app/HumanStateVisualizer_iRonCub_openxr_ifeel.ini @@ -0,0 +1,26 @@ +name HumanStateVisualizer + +# Model Configuration options +modelURDFName "model.urdf" +ignoreMissingLinks true +visualizeWrenches false +visualizeFrames true +visualizeTargets true + +# Camera options +cameraDeltaPosition (0.0, 2.0, 0.5) +useFixedCamera true # if set to false, the camera follows the model base link +fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false +maxVisualizationFPS 65 + +# Link visualization option +visualizedLinksFrame (root_link l_shoulder_3 r_shoulder_3 head chest) +linksFrameScalingFactor 0.1 + +# Targets visualization option +visualizedTargetsFrame ( target_RightHand target_LeftHand target_Pelvis target_Head target_Chest target_r_upper_arm target_l_upper_arm) +targetsFrameScalingFactor 0.2 + +# Client Configuration +humanStateDataPortName "/iCub/RobotStateServer/state:o" +wearableTargetsServerPortName "/iCub/HDE/WearableTargetsServer/state:o" diff --git a/conf/xml/RobotStateProvider_iCub2_5_openxr.xml b/conf/xml/RobotStateProvider_iCub2_5_openxr.xml index f7bdc30a..45cba310 100644 --- a/conf/xml/RobotStateProvider_iCub2_5_openxr.xml +++ b/conf/xml/RobotStateProvider_iCub2_5_openxr.xml @@ -2,14 +2,44 @@ + + 0.01 + ftc_yarp_only.xml + /icub/IFrameTransformToIWear/tf + /icub/IFrameTransformToIWear/tf/local_rpc + 10.0 + + + + TransformServer + iCub_robots_base_link + PoseSensor + (iCub_robots_base_link + openxr_head + vive_tracker_right_elbow_pose + vive_tracker_left_elbow_pose) + + + TransformClient + + + + + - (/Wearable/OpenXRTransform/data:o) + () + false + + + IFrameTransformToIWear + + 0.01 - teleoperation_iCub_model_V_2_5.urdf - chest_fake + model.urdf + root_link dynamical true @@ -42,11 +72,10 @@ (700.0 20.0) (0.0 0.0) - (root_link_fake, TransformServer::pose::virtualizer_frame, orientation) - (chest_fake, TransformServer::pose::virtualizer_frame, pose) - (r_hand_fake, TransformServer::pose::/trackers/LHR-3C2E1582, pose) - (l_hand_fake, TransformServer::pose::/trackers/LHR-8F3A4D72, pose) - (head_fake, TransformServer::pose::/hmd/LHR-752422B9, pose) + (root_link, TransformServer::pose::iCub_robots_base_link, orientation) + (r_hand, TransformServer::pose::vive_tracker_right_elbow_pose, pose) + (l_hand, TransformServer::pose::vive_tracker_left_elbow_pose, pose) + (head, TransformServer::pose::openxr_head, pose) ("l_shoulder_pitch", "l_shoulder_roll", @@ -62,40 +91,34 @@ "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", - "j_l_upper_arm_fake", - "j_r_upper_arm_fake", - "j_l_forearm_fake", - "j_r_forearm_fake", - "j_l_hand_fake", - "j_r_hand_fake", "torso_pitch", "torso_roll", "torso_yaw", "neck_pitch", "neck_roll", - "neck_yaw", - "j_chest_fake", - "j_root_link_fake", - "j_head_fake") + "neck_yaw") - ( 1.0 0.0 0.0 0.0 - 0.0 -1.0 0.0 0.025 - 0.0 0.0 -1.0 0.11 + ( 0.0 0.0 1.0 0.0 + -1.0 0.0 0.0 -0.1 + 0.0 -1.0 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.025 - 0.0 0.0 -1.0 0.11 + ( 0.0 0.0 -1.0 0.0 + 1.0 0.0 0.0 -0.1 + 0.0 -1.0 0.0 0.0 0.0 0.0 0.0 1.0) - ( 0.0 -1.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - -1.0 0.0 0.0 0.1 + (-1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 -0.2 + 0.0 0.0 -1.0 0.2 0.0 0.0 0.0 1.0) ( 0.7 0.7 0.6 ) ( 0.7 0.7 0.6 ) ( 0.7 0.7 0.6 ) + 0.7 + 0.7 + 0.6 @@ -107,8 +130,8 @@ (2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 1.0, 1.0, 1.0) - (0.0, 0.0, 0.0, 1.0, 1.0, 1.0 ) - (0.0, 0.0, 0.0, -1.0, -1.0, -1.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 ) ( @@ -158,10 +181,10 @@ - + 0.01 - /HDE/WearableTargetsServer/state:o + /iCub/HDE/WearableTargetsServer/state:o RobotStateProvider diff --git a/conf/xml/RobotStateProvider_iCub2_5_openxr_ifeel.xml b/conf/xml/RobotStateProvider_iCub2_5_openxr_ifeel.xml index d86daeef..74c7ea94 100644 --- a/conf/xml/RobotStateProvider_iCub2_5_openxr_ifeel.xml +++ b/conf/xml/RobotStateProvider_iCub2_5_openxr_ifeel.xml @@ -2,14 +2,43 @@ + + 0.01 + ftc_yarp_only.xml + /icub/IFrameTransformToIWear/tf + /icub/IFrameTransformToIWear/tf/local_rpc + 10.0 + + + + TransformServer + iCub_robots_base_link + PoseSensor + (iCub_robots_base_link + openxr_head + vive_tracker_right_elbow_pose + vive_tracker_left_elbow_pose) + + + TransformClient + + + + + - (/Wearable/OpenXRTransform/data:o /iFeelSuit/WearableData/data:o) + (/iFeelSuit/WearableData/data:o) + + + IFrameTransformToIWear + + 0.01 - teleoperation_iCub_model_V_2_5.urdf - chest_fake + model.urdf + root_link dynamical true @@ -42,15 +71,15 @@ (700.0 20.0) (0.0 0.0) - (root_link_fake, TransformServer::pose::virtualizer_frame, orientation) - (chest_fake, TransformServer::pose::virtualizer_frame, pose) - (r_hand_fake, TransformServer::pose::/trackers/LHR-3C2E1582, pose) - (l_hand_fake, TransformServer::pose::/trackers/LHR-8F3A4D72, pose) - (head_fake, TransformServer::pose::/hmd/LHR-752422B9, pose) - (l_upper_arm_fake iFeelSuit::vLink::Node#5, orientationAndVelocity) - (l_forearm_fake, iFeelSuit::vLink::Node#4, orientationAndVelocity) - (r_upper_arm_fake iFeelSuit::vLink::Node#7, orientationAndVelocity) - (r_forearm_fake, iFeelSuit::vLink::Node#8, orientationAndVelocity) + (root_link, TransformServer::pose::iCub_robots_base_link, pose) + (r_hand, TransformServer::pose::vive_tracker_right_elbow_pose, pose) + (l_hand, TransformServer::pose::vive_tracker_left_elbow_pose, pose) + (head, TransformServer::pose::openxr_head, pose) + (chest, iFeelSuit::vLink::Node#6, gravity) + (l_shoulder_3 iFeelSuit::vLink::Node#5, gravity) + (l_forearm, iFeelSuit::vLink::Node#4, gravity) + (r_shoulder_3 iFeelSuit::vLink::Node#7, gravity) + (r_forearm, iFeelSuit::vLink::Node#8, gravity) ("l_shoulder_pitch", "l_shoulder_roll", @@ -66,35 +95,46 @@ "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", - "j_l_upper_arm_fake", - "j_r_upper_arm_fake", - "j_l_forearm_fake", - "j_r_forearm_fake", - "j_l_hand_fake", - "j_r_hand_fake", "torso_pitch", "torso_roll", "torso_yaw", "neck_pitch", "neck_roll", - "neck_yaw", - "j_chest_fake", - "j_root_link_fake", - "j_head_fake") + "neck_yaw") - ( 1.0 0.0 0.0 0.0 - 0.0 -1.0 0.0 0.025 - 0.0 0.0 -1.0 0.11 + ( 0.0 0.0 1.0 0.0 + -1.0 0.0 0.0 -0.1 + 0.0 -1.0 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.025 - 0.0 0.0 -1.0 0.11 + ( 0.0 0.0 -1.0 0.0 + 1.0 0.0 0.0 -0.1 + 0.0 -1.0 0.0 0.0 0.0 0.0 0.0 1.0) - ( 0.0 -1.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - -1.0 0.0 0.0 0.1 + (-1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 -0.2 + 0.0 0.0 -1.0 0.2 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 1.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) ( 0.7 0.7 0.6 ) @@ -114,8 +154,8 @@ (2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 1.0, 1.0, 1.0) - (0.0, 0.0, 0.0, 1.0, 1.0, 1.0 ) - (0.0, 0.0, 0.0, -1.0, -1.0, -1.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 ) ( @@ -165,10 +205,10 @@ - + 0.01 - /HDE/WearableTargetsServer/state:o + /iCub/HDE/WearableTargetsServer/state:o RobotStateProvider diff --git a/conf/xml/RobotStateProvider_iRonCub_openxr.xml b/conf/xml/RobotStateProvider_iRonCub_openxr.xml new file mode 100644 index 00000000..e4f09b1d --- /dev/null +++ b/conf/xml/RobotStateProvider_iRonCub_openxr.xml @@ -0,0 +1,184 @@ + + + + + + 0.01 + ftc_yarp_only.xml + /icub/IFrameTransformToIWear/tf + /icub/IFrameTransformToIWear/tf/local_rpc + 10.0 + + + + TransformServer + iCub_robots_base_link + PoseSensor + (iCub_robots_base_link + openxr_head + vive_tracker_right_elbow_pose + vive_tracker_left_elbow_pose) + + + TransformClient + + + + + + + () + false + + + IFrameTransformToIWear + + + + + + 0.01 + model.urdf + root_link + + dynamical + true + false + + 300 + ma27 + 2 + 0.0 + 1.0 + 1.0 + 0.001 + iCub + + + QP + 1.0 + 1.0 + + 10.0 + (1.0 1.0) + (700.0 20.0) + (0.0 0.0) + + (root_link, TransformServer::pose::iCub_robots_base_link, orientation) + (r_elbow_1, TransformServer::pose::vive_tracker_right_elbow_pose, pose) + (l_elbow_1, TransformServer::pose::vive_tracker_left_elbow_pose, pose) + (head, TransformServer::pose::openxr_head, pose) + + ("l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "torso_pitch", + "torso_roll", + "torso_yaw", + "neck_pitch", + "neck_roll", + "neck_yaw") + + + ( 0.0 0.0 1.0 0.0 + -1.0 0.0 0.0 0.1 + 0.0 -1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 0.0 -1.0 0.0 + 1.0 0.0 0.0 0.1 + 0.0 -1.0 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.2 + 0.0 0.0 -1.0 0.2 + 0.0 0.0 0.0 1.0) + + + ( 0.7 0.7 0.6 ) + ( 0.7 0.7 0.6 ) + ( 0.7 0.7 0.6 ) + 0.7 + 0.7 + 0.6 + + + + + + + (l_shoulder_pitch, l_shoulder_roll, l_shoulder_yaw, r_shoulder_pitch, r_shoulder_roll, r_shoulder_yaw, torso_roll, torso_pitch, torso_yaw) + + + (2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 1.0, 1.0, 1.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 ) + + + ( + l_shoulder_pitch, l_shoulder_roll, l_shoulder_yaw, r_shoulder_pitch, r_shoulder_roll, r_shoulder_yaw) + ( + (1.7105, -1.7105, 0.0, 0.0, 0.0, 0.0, ), + (1.7105, -1.7105, -1.7105, 0.0, 0.0, 0.0, ), + (0.0, 1.0, 1.0, 0.0, 0.0, 0.0, ), + (0.0, 1.0, 0.0427, 0.0, 0.0, 0.0, ), + (0.0, 1.0, 0.0, 0.0, 0.0, 0.0, ), + (0.0, 0.0, 0.0, 1.7105, -1.7105, 0.0, ), + (0.0, 0.0, 0.0, 1.7105, -1.7105, -1.7105, ), + (0.0, 0.0, 0.0, 0.0, 1.0, 1.0, ), + (0.0, 0.0, 0.0, 0.0, 1.0, 0.0427, ), + (0.0, 0.0, 0.0, 0.0, 1.0, 0.0, ), + ) + ( + -6.0563, -6.3979, -1.1623, 0.4611, 0.5, + -6.0563, -6.3979, -1.1623, 0.4611, 0.5,) + ( + 1000.0, 1.9622, 3.7228, 1000.0, 1.7453, + 1000.0, 1.9622, 3.7228, 1000.0, 1.7453,) + 0.5 + + + + XSenseIWearRemapper + + + + + + + + 0.01 + /iCub/RobotStateServer/state:o + + + RobotStateProvider + + + + + + + 0.01 + /iCub/HDE/WearableTargetsServer/state:o + + + RobotStateProvider + + + + + + diff --git a/conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml b/conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml new file mode 100644 index 00000000..5e871f35 --- /dev/null +++ b/conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml @@ -0,0 +1,198 @@ + + + + + + 0.01 + ftc_yarp_only.xml + /icub/IFrameTransformToIWear/tf + /icub/IFrameTransformToIWear/tf/local_rpc + 10.0 + + + + TransformServer + iCub_robots_base_link + PoseSensor + (iCub_robots_base_link + openxr_head + vive_tracker_right_elbow_pose + vive_tracker_left_elbow_pose) + + + TransformClient + + + + + + + (/iFeelSuit/WearableData/data:o) + + + IFrameTransformToIWear + + + + + + 0.01 + model.urdf + root_link + + dynamical + true + false + + 300 + ma27 + 2 + 0.0 + 1.0 + 1.0 + 0.001 + iCub + + + QP + 1.0 + 1.0 + + 10.0 + (1.0 1.0) + (700.0 20.0) + (0.0 0.0) + + (root_link, TransformServer::pose::iCub_robots_base_link, orientation) + (r_elbow_1, TransformServer::pose::vive_tracker_right_elbow_pose, pose) + (l_elbow_1, TransformServer::pose::vive_tracker_left_elbow_pose, pose) + (head, TransformServer::pose::openxr_head, pose) + (chest, iFeelSuit::vLink::Node#6, gravity) + (l_shoulder_3 iFeelSuit::vLink::Node#5, gravity) + (r_shoulder_3 iFeelSuit::vLink::Node#7, gravity) + + ("l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "torso_pitch", + "torso_roll", + "torso_yaw", + "neck_pitch", + "neck_roll", + "neck_yaw") + + + ( 0.0 0.0 1.0 0.0 + -1.0 0.0 0.0 0.1 + 0.0 -1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 0.0 -1.0 0.0 + 1.0 0.0 0.0 0.1 + 0.0 -1.0 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.2 + 0.0 0.0 -1.0 0.2 + 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 1.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + + + ( 0.7 0.7 0.6 ) + ( 0.7 0.7 0.6 ) + ( 0.7 0.7 0.6 ) + 0.7 + 0.7 + 0.6 + + + + + + + (l_shoulder_pitch, l_shoulder_roll, l_shoulder_yaw, r_shoulder_pitch, r_shoulder_roll, r_shoulder_yaw, torso_roll, torso_pitch, torso_yaw) + + + (2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 1.0, 1.0, 1.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 ) + + + ( + l_shoulder_pitch, l_shoulder_roll, l_shoulder_yaw, r_shoulder_pitch, r_shoulder_roll, r_shoulder_yaw) + ( + (1.7105, -1.7105, 0.0, 0.0, 0.0, 0.0, ), + (1.7105, -1.7105, -1.7105, 0.0, 0.0, 0.0, ), + (0.0, 1.0, 1.0, 0.0, 0.0, 0.0, ), + (0.0, 1.0, 0.0427, 0.0, 0.0, 0.0, ), + (0.0, 1.0, 0.0, 0.0, 0.0, 0.0, ), + (0.0, 0.0, 0.0, 1.7105, -1.7105, 0.0, ), + (0.0, 0.0, 0.0, 1.7105, -1.7105, -1.7105, ), + (0.0, 0.0, 0.0, 0.0, 1.0, 1.0, ), + (0.0, 0.0, 0.0, 0.0, 1.0, 0.0427, ), + (0.0, 0.0, 0.0, 0.0, 1.0, 0.0, ), + ) + ( + -6.0563, -6.3979, -1.1623, 0.4611, 0.5, + -6.0563, -6.3979, -1.1623, 0.4611, 0.5,) + ( + 1000.0, 1.9622, 3.7228, 1000.0, 1.7453, + 1000.0, 1.9622, 3.7228, 1000.0, 1.7453,) + 0.5 + + + + XSenseIWearRemapper + + + + + + + + 0.01 + /iCub/RobotStateServer/state:o + + + RobotStateProvider + + + + + + + 0.01 + /iCub/HDE/WearableTargetsServer/state:o + + + RobotStateProvider + + + + + + From 801eb16d69b42b25fc2395090d52b099ad7f73de Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Tue, 24 Sep 2024 12:42:16 +0200 Subject: [PATCH 07/11] Updated configuration files for iRonCub IK --- conf/xml/RobotStateProvider_iRonCub_openxr.xml | 2 +- conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/conf/xml/RobotStateProvider_iRonCub_openxr.xml b/conf/xml/RobotStateProvider_iRonCub_openxr.xml index e4f09b1d..0dae338a 100644 --- a/conf/xml/RobotStateProvider_iRonCub_openxr.xml +++ b/conf/xml/RobotStateProvider_iRonCub_openxr.xml @@ -103,7 +103,7 @@ 0.0 0.0 0.0 1.0) (-1.0 0.0 0.0 0.0 0.0 1.0 0.0 -0.2 - 0.0 0.0 -1.0 0.2 + 0.0 0.0 -1.0 0.0 0.0 0.0 0.0 1.0) diff --git a/conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml b/conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml index 5e871f35..af8c6829 100644 --- a/conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml +++ b/conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml @@ -105,7 +105,7 @@ 0.0 0.0 0.0 1.0) (-1.0 0.0 0.0 0.0 0.0 1.0 0.0 -0.2 - 0.0 0.0 -1.0 0.2 + 0.0 0.0 -1.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 From 41cb71e42e6bac59226492eed25ab4227f9ccd59 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Mon, 30 Sep 2024 16:21:14 +0200 Subject: [PATCH 08/11] Changed the role of the trackers when using joystick and trackers --- ...obotStateProvider_ergoCub_openxr_joystick_trackers.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr_joystick_trackers.xml b/conf/xml/RobotStateProvider_ergoCub_openxr_joystick_trackers.xml index 55bf8e23..04989934 100644 --- a/conf/xml/RobotStateProvider_ergoCub_openxr_joystick_trackers.xml +++ b/conf/xml/RobotStateProvider_ergoCub_openxr_joystick_trackers.xml @@ -18,8 +18,8 @@ openxr_head openxr_right_hand openxr_left_hand - vive_tracker_right_elbow_pose - vive_tracker_left_elbow_pose) + vive_tracker_right_shoulder_pose + vive_tracker_left_shoulder_pose) TransformClient @@ -78,8 +78,8 @@ (r_hand_palm, TransformServer::pose::openxr_right_hand, pose) (l_hand_palm, TransformServer::pose::openxr_left_hand, pose) (head, TransformServer::pose::openxr_head, pose) - (r_upperarm TransformServer::pose::vive_tracker_right_elbow_pose, orientation) - (l_upperarm TransformServer::pose::vive_tracker_left_elbow_pose, orientation) + (r_upperarm TransformServer::pose::vive_tracker_right_shoulder_pose, orientation) + (l_upperarm TransformServer::pose::vive_tracker_left_shoulder_pose, orientation) ("l_shoulder_pitch", "l_shoulder_roll", From c0b0479a2794d7d441ff269ecb1f5d95b130098e Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Wed, 16 Oct 2024 15:06:58 +0200 Subject: [PATCH 09/11] Using left_glove and right_glove in the hand transforms in 5 trackers configurations Added 5 trackers configuration for iRonCub --- ...StateProvider_ergoCub_openxr_5trackers.xml | 8 +- ...StateProvider_iRonCub_openxr_5trackers.xml | 196 ++++++++++++++++++ 2 files changed, 200 insertions(+), 4 deletions(-) create mode 100644 conf/xml/RobotStateProvider_iRonCub_openxr_5trackers.xml diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml b/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml index 472c0a15..d2eda047 100644 --- a/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml +++ b/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml @@ -16,8 +16,8 @@ PoseSensor (root_link_desired openxr_head - vive_tracker_right_elbow_pose - vive_tracker_left_elbow_pose + right_glove + left_glove vive_tracker_right_shoulder_pose vive_tracker_left_shoulder_pose) @@ -75,8 +75,8 @@ (0.0 0.0) (root_link, TransformServer::pose::root_link_desired, orientation) - (r_hand_palm, TransformServer::pose::vive_tracker_right_elbow_pose, pose) - (l_hand_palm, TransformServer::pose::vive_tracker_left_elbow_pose, pose) + (r_hand_palm, TransformServer::pose::right_glove, pose) + (l_hand_palm, TransformServer::pose::left_glove, pose) (head, TransformServer::pose::openxr_head, pose) (r_upper_arm TransformServer::pose::vive_tracker_right_shoulder_pose, orientation) (l_upper_arm TransformServer::pose::vive_tracker_left_shoulder_pose, orientation) diff --git a/conf/xml/RobotStateProvider_iRonCub_openxr_5trackers.xml b/conf/xml/RobotStateProvider_iRonCub_openxr_5trackers.xml new file mode 100644 index 00000000..7b6d8edd --- /dev/null +++ b/conf/xml/RobotStateProvider_iRonCub_openxr_5trackers.xml @@ -0,0 +1,196 @@ + + + + + + 0.01 + ftc_yarp_only.xml + /icub/IFrameTransformToIWear/tf + /icub/IFrameTransformToIWear/tf/local_rpc + 10.0 + + + + TransformServer + iCub_robots_base_link + PoseSensor + (iCub_robots_base_link + openxr_head + right_glove + left_glove + vive_tracker_right_shoulder_pose + vive_tracker_left_shoulder_pose) + + + TransformClient + + + + + + + () + false + + + IFrameTransformToIWear + + + + + + 0.01 + model.urdf + root_link + + dynamical + true + false + + 300 + ma27 + 2 + 0.0 + 1.0 + 1.0 + 0.001 + iCub + + + QP + 1.0 + 1.0 + + 10.0 + (1.0 1.0) + (700.0 20.0) + (0.0 0.0) + + (root_link, TransformServer::pose::iCub_robots_base_link, orientation) + (r_elbow_1, TransformServer::pose::right_glove, pose) + (l_elbow_1, TransformServer::pose::left_glove, pose) + (r_shoulder_3 TransformServer::pose::vive_tracker_right_shoulder_pose, orientation) + (l_shoulder_3 TransformServer::pose::vive_tracker_left_shoulder_pose, orientation) + (head, TransformServer::pose::openxr_head, pose) + + ("l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "torso_pitch", + "torso_roll", + "torso_yaw", + "neck_pitch", + "neck_roll", + "neck_yaw") + + + ( 0.0 0.0 1.0 0.0 + -1.0 0.0 0.0 0.1 + 0.0 -1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 0.0 -1.0 0.0 + 1.0 0.0 0.0 0.1 + 0.0 -1.0 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.2 + 0.0 0.0 -1.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 0.0 -1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 -1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 0.0 1.0 0.0 + -1.0 0.0 0.0 0.0 + 0.0 -1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + + + ( 0.7 0.7 0.6 ) + ( 0.7 0.7 0.6 ) + ( 0.7 0.7 0.6 ) + 0.7 + 0.7 + 0.6 + + + + + + + (l_shoulder_pitch, l_shoulder_roll, l_shoulder_yaw, r_shoulder_pitch, r_shoulder_roll, r_shoulder_yaw, torso_roll, torso_pitch, torso_yaw) + + + (2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 1.0, 1.0, 1.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 ) + + + ( + l_shoulder_pitch, l_shoulder_roll, l_shoulder_yaw, r_shoulder_pitch, r_shoulder_roll, r_shoulder_yaw) + ( + (1.7105, -1.7105, 0.0, 0.0, 0.0, 0.0, ), + (1.7105, -1.7105, -1.7105, 0.0, 0.0, 0.0, ), + (0.0, 1.0, 1.0, 0.0, 0.0, 0.0, ), + (0.0, 1.0, 0.0427, 0.0, 0.0, 0.0, ), + (0.0, 1.0, 0.0, 0.0, 0.0, 0.0, ), + (0.0, 0.0, 0.0, 1.7105, -1.7105, 0.0, ), + (0.0, 0.0, 0.0, 1.7105, -1.7105, -1.7105, ), + (0.0, 0.0, 0.0, 0.0, 1.0, 1.0, ), + (0.0, 0.0, 0.0, 0.0, 1.0, 0.0427, ), + (0.0, 0.0, 0.0, 0.0, 1.0, 0.0, ), + ) + ( + -6.0563, -6.3979, -1.1623, 0.4611, 0.5, + -6.0563, -6.3979, -1.1623, 0.4611, 0.5,) + ( + 1000.0, 1.9622, 3.7228, 1000.0, 1.7453, + 1000.0, 1.9622, 3.7228, 1000.0, 1.7453,) + 0.5 + + + + XSenseIWearRemapper + + + + + + + + 0.01 + /iCub/RobotStateServer/state:o + + + RobotStateProvider + + + + + + + 0.01 + /iCub/HDE/WearableTargetsServer/state:o + + + RobotStateProvider + + + + + + From 49d3e170aad2b4885e8dc4942a2159b8ec4e20f4 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Wed, 16 Oct 2024 17:05:54 +0200 Subject: [PATCH 10/11] Added option to control base height when using 5 trackers --- ...RobotStateProvider_ergoCub_openxr_5trackers.xml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml b/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml index d2eda047..1a1ebef1 100644 --- a/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml +++ b/conf/xml/RobotStateProvider_ergoCub_openxr_5trackers.xml @@ -7,12 +7,12 @@ ftc_yarp_only.xml /IFrameTransformToIWear/tf /IFrameTransformToIWear/tf/local_rpc - 1.0 + 10.0 TransformServer - root_link_desired + world_grounded PoseSensor (root_link_desired openxr_head @@ -71,7 +71,7 @@ 10.0 (1.0 1.0) - (700.0 20.0) + (200.0 20.0) (0.0 0.0) (root_link, TransformServer::pose::root_link_desired, orientation) @@ -138,13 +138,13 @@ - ( ) + () - ( ) + () - (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.2, 0.0, 0.0, 0.0) + (0.0, 0.0, -0.2, 0.0, 0.0, 0.0) ( From 8063532db8d5983579521ab95cd8817d09cf6882 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 29 Nov 2024 14:32:42 +0100 Subject: [PATCH 11/11] Using right_ and left_glove instead of the actual tracker poses in ergoCub and iRonCub --- conf/xml/RobotStateProvider_ergoCub_openxr.xml | 8 ++++---- ...obotStateProvider_ergoCub_openxr_5ultimateTrackers.xml | 8 ++++---- conf/xml/RobotStateProvider_ergoCub_openxr_ifeel.xml | 8 ++++---- conf/xml/RobotStateProvider_iRonCub_openxr.xml | 8 ++++---- conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml | 8 ++++---- 5 files changed, 20 insertions(+), 20 deletions(-) diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr.xml b/conf/xml/RobotStateProvider_ergoCub_openxr.xml index 64620c2b..e224bc5d 100644 --- a/conf/xml/RobotStateProvider_ergoCub_openxr.xml +++ b/conf/xml/RobotStateProvider_ergoCub_openxr.xml @@ -16,8 +16,8 @@ PoseSensor (root_link_desired openxr_head - vive_tracker_right_elbow_pose - vive_tracker_left_elbow_pose) + right_glove + left_glove) TransformClient @@ -73,8 +73,8 @@ (0.0 0.0) (root_link, TransformServer::pose::root_link_desired, orientation) - (r_hand_palm, TransformServer::pose::vive_tracker_right_elbow_pose, pose) - (l_hand_palm, TransformServer::pose::vive_tracker_left_elbow_pose, pose) + (r_hand_palm, TransformServer::pose::right_glove, pose) + (l_hand_palm, TransformServer::pose::left_glove, pose) (head, TransformServer::pose::openxr_head, pose) ("l_shoulder_pitch", diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr_5ultimateTrackers.xml b/conf/xml/RobotStateProvider_ergoCub_openxr_5ultimateTrackers.xml index d78de9f1..1e66c06c 100644 --- a/conf/xml/RobotStateProvider_ergoCub_openxr_5ultimateTrackers.xml +++ b/conf/xml/RobotStateProvider_ergoCub_openxr_5ultimateTrackers.xml @@ -16,8 +16,8 @@ PoseSensor (world_ut openxr_head - vive_tracker_right_elbow_pose - vive_tracker_left_elbow_pose + right_glove + left_glove vive_tracker_right_shoulder_pose vive_tracker_left_shoulder_pose) @@ -75,8 +75,8 @@ (0.0 0.0) (root_link, TransformServer::pose::world_ut, orientation) - (r_hand_palm, TransformServer::pose::vive_tracker_right_elbow_pose, pose) - (l_hand_palm, TransformServer::pose::vive_tracker_left_elbow_pose, pose) + (r_hand_palm, TransformServer::pose::right_glove, pose) + (l_hand_palm, TransformServer::pose::left_glove, pose) (head, TransformServer::pose::openxr_head, pose) (r_upper_arm TransformServer::pose::vive_tracker_right_shoulder_pose, orientation) (l_upper_arm TransformServer::pose::vive_tracker_left_shoulder_pose, orientation) diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr_ifeel.xml b/conf/xml/RobotStateProvider_ergoCub_openxr_ifeel.xml index 91583cf9..648e3078 100644 --- a/conf/xml/RobotStateProvider_ergoCub_openxr_ifeel.xml +++ b/conf/xml/RobotStateProvider_ergoCub_openxr_ifeel.xml @@ -16,8 +16,8 @@ PoseSensor (root_link_desired openxr_head - vive_tracker_right_elbow_pose - vive_tracker_left_elbow_pose) + right_glove + left_glove) TransformClient @@ -73,8 +73,8 @@ (root_link, TransformServer::pose::root_link_desired, pose) (chest, iFeelSuit::vLink::Node#6, gravity) - (r_hand_palm, TransformServer::pose::vive_tracker_right_elbow_pose, pose) - (l_hand_palm, TransformServer::pose::vive_tracker_left_elbow_pose, pose) + (r_hand_palm, TransformServer::pose::right_glove, pose) + (l_hand_palm, TransformServer::pose::left_glove, pose) (head, TransformServer::pose::openxr_head, pose) (l_upper_arm iFeelSuit::vLink::Node#5, gravity) (l_forearm, iFeelSuit::vLink::Node#4, gravity) diff --git a/conf/xml/RobotStateProvider_iRonCub_openxr.xml b/conf/xml/RobotStateProvider_iRonCub_openxr.xml index 0dae338a..ca93bff4 100644 --- a/conf/xml/RobotStateProvider_iRonCub_openxr.xml +++ b/conf/xml/RobotStateProvider_iRonCub_openxr.xml @@ -16,8 +16,8 @@ PoseSensor (iCub_robots_base_link openxr_head - vive_tracker_right_elbow_pose - vive_tracker_left_elbow_pose) + right_glove + left_glove) TransformClient @@ -73,8 +73,8 @@ (0.0 0.0) (root_link, TransformServer::pose::iCub_robots_base_link, orientation) - (r_elbow_1, TransformServer::pose::vive_tracker_right_elbow_pose, pose) - (l_elbow_1, TransformServer::pose::vive_tracker_left_elbow_pose, pose) + (r_elbow_1, TransformServer::pose::right_glove, pose) + (l_elbow_1, TransformServer::pose::left_glove, pose) (head, TransformServer::pose::openxr_head, pose) ("l_shoulder_pitch", diff --git a/conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml b/conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml index af8c6829..870054b5 100644 --- a/conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml +++ b/conf/xml/RobotStateProvider_iRonCub_openxr_ifeel.xml @@ -16,8 +16,8 @@ PoseSensor (iCub_robots_base_link openxr_head - vive_tracker_right_elbow_pose - vive_tracker_left_elbow_pose) + right_glove + left_glove) TransformClient @@ -72,8 +72,8 @@ (0.0 0.0) (root_link, TransformServer::pose::iCub_robots_base_link, orientation) - (r_elbow_1, TransformServer::pose::vive_tracker_right_elbow_pose, pose) - (l_elbow_1, TransformServer::pose::vive_tracker_left_elbow_pose, pose) + (r_elbow_1, TransformServer::pose::right_glove, pose) + (l_elbow_1, TransformServer::pose::left_glove, pose) (head, TransformServer::pose::openxr_head, pose) (chest, iFeelSuit::vLink::Node#6, gravity) (l_shoulder_3 iFeelSuit::vLink::Node#5, gravity)