From fd1014886b7ab81261cd808dac4775ea94babb0f Mon Sep 17 00:00:00 2001
From: Lorenzo <lor.rapetti@gmail.com>
Date: Thu, 29 Jun 2023 18:40:12 +0200
Subject: [PATCH] Add ergoCub openxr teleoperation configuration files

---
 .../HumanStateVisualizer_ergoCub_openxr.ini   |  26 +++
 .../xml/RobotStateProvider_ergoCub_openxr.xml | 182 ++++++++++++++++++
 2 files changed, 208 insertions(+)
 create mode 100644 conf/app/HumanStateVisualizer_ergoCub_openxr.ini
 create mode 100644 conf/xml/RobotStateProvider_ergoCub_openxr.xml

diff --git a/conf/app/HumanStateVisualizer_ergoCub_openxr.ini b/conf/app/HumanStateVisualizer_ergoCub_openxr.ini
new file mode 100644
index 000000000..4faa84442
--- /dev/null
+++ b/conf/app/HumanStateVisualizer_ergoCub_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 (r_hand_palm l_hand_palm root_link head) 
+linksFrameScalingFactor   0.1
+
+# Targets visualization option
+visualizedTargetsFrame ( target_RightHand target_LeftHand target_Pelvis target_Head)
+targetsFrameScalingFactor  0.2
+
+# Remapper Configuration
+humanStateDataPortName "/ergoCub/RobotStateWrapper/state:o"
+wearableTargetsWrapperPortName "/HDE/WearableTargetsWrapper/state:o"
diff --git a/conf/xml/RobotStateProvider_ergoCub_openxr.xml b/conf/xml/RobotStateProvider_ergoCub_openxr.xml
new file mode 100644
index 000000000..eb947693d
--- /dev/null
+++ b/conf/xml/RobotStateProvider_ergoCub_openxr.xml
@@ -0,0 +1,182 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+<robot name="ergoCub-Retargeting" build=0 portprefix="">
+
+    <device type="transformClient" name="TransformClient">
+        <param name="period">0.01</param>
+        <param name="local">/tf</param>
+        <param name="remote">/transformServer</param>
+    </device>
+
+    <device type="iframetransform_to_iwear" name="IFrameTransformToIWear">
+        <param name="wearableName">TransformServer</param>
+        <param extern-name="rootFrame" name="rootFrameID">root_link_desired</param>
+        <param name="wearableSensorType">PoseSensor</param>
+        <param extern-name="frames" name="frameIDs">(root_link_desired
+                                                     openxr_head
+                                                     vive_tracker_right_elbow_pose
+                                                     vive_tracker_left_elbow_pose)</param>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <elem name="IFrameTransformToIWearLabel">TransformClient</elem>
+           </paramlist>
+        </action>
+        <action phase="shutdown" level="5" type="detach"/>
+    </device>
+
+    <device type="iwear_remapper" name="XSenseIWearRemapper">
+        <param name="wearableDataPorts">()</param>
+        <param name="useRPC">false</param>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <elem name="IFrameTransformToIWear">IFrameTransformToIWear</elem>
+            </paramlist>
+        </action>
+    </device>
+
+    <device type="human_state_provider" name="RobotStateProvider">
+        <param name="period">0.01</param>
+        <param name="urdf">model.urdf</param>
+        <param name="floatingBaseFrame">root_link</param>
+        <!-- ikSolver options: pairwised, global, dynamical -->
+        <param name="ikSolver">dynamical</param>
+        <param name="allowIKFailures">true</param>
+        <param name="useDirectBaseMeasurement">false</param>
+        <!-- optimization parameters -->
+        <param name="maxIterationsIK">300</param>
+        <param name="ikLinearSolver">ma27</param>
+        <param name="ikPoolSizeOption">2</param>
+        <param name="posTargetWeight">0.0</param>
+        <param name="rotTargetWeight">1.0</param>
+        <param name="costRegularization">1.0</param>
+        <param name="costTolerance">0.001</param>
+        <param name="rpcPortPrefix">ergoCub</param>
+        <!-- inverse velocity kinematics parameters -->
+        <!-- inverseVelocityKinematicsSolver values:
+        QP
+        moorePenrose,
+        completeOrthogonalDecomposition,
+        leastSquare,
+        choleskyDecomposition,
+        sparseCholeskyDecomposition,
+        robustCholeskyDecomposition,
+        sparseRobustCholeskyDecomposition -->
+        <param name='inverseVelocityKinematicsSolver'>QP</param>
+        <param name="linVelTargetWeight">1.0</param>
+        <param name="angVelTargetWeight">1.0</param>
+        <!-- integration based IK parameters -->
+        <param name='dynamicalIKJointVelocityLimit'>10.0</param> <!-- comment or -1.0 for no limits -->
+        <param name="dynamicalIKMeasuredVelocityGainLinRot">(1.0 1.0)</param>
+        <param name="dynamicalIKCorrectionGainsLinRot">(700.0 20.0)</param>
+        <param name="dynamicalIKIntegralCorrectionGainsLinRot">(0.0 0.0)</param>
+        <group name="WEARABLE_SENSOR_TARGETS">
+            <param name="target_Pelvis">(root_link, TransformServer::pose::root_link_desired, orientation)</param>
+            <param name="target_RightHand">(r_hand_palm, TransformServer::pose::vive_tracker_right_elbow_pose, pose)</param>
+            <param name="target_LeftHand">(l_hand_palm, TransformServer::pose::vive_tracker_left_elbow_pose, pose)</param>
+            <param name="target_Head">(head, TransformServer::pose::openxr_head, pose)</param>
+        </group>
+        <param name="jointList">("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")
+        </param>
+        <group name="MEASUREMENT_TO_LINK_TRANSFORMS">
+            <param name="target_LeftHand">( 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)</param>
+            <param name="target_RightHand">(-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)</param>
+            <param name="target_Head">( 0.0 -1.0  0.0  0.0
+                                        0.0  0.0  1.0  0.0
+                                       -1.0  0.0  0.0  0.1
+                                        0.0  0.0  0.0  1.0)</param>
+            <param name="target_Chest">( 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  0.0  0.0  1.0)</param>
+        </group>
+        <group name="MEASUREMENT_POSITION_SCALE_FACTOR">
+            <param name="target_Head">( 0.7 0.7 0.6 )</param>
+            <param name="target_LeftHand">( 0.7 0.7 0.6 )</param>
+            <param name="target_RightHand">( 0.7 0.7 0.6 )</param>
+            <param name="x_scale_factor_all" extern-name="xy_scale">0.7</param>
+            <param name="y_scale_factor_all" extern-name="xy_scale">0.7</param>
+            <param name="z_scale_factor_all" extern-name="z_scale">0.7</param>
+        </group>
+        <group name="CUSTOM_CONSTRAINTS">
+        <!-- check issue https://github.com/robotology/human-dynamics-estimation/issues/132 for more info-->
+        <!-- note that a group can not be empty, otherwise it returns error-->
+        <!-- custom joint limits velocities-->
+            <!--param name="custom_joints_velocity_limits_names">(neck_roll, neck_pitch)</param-->
+            <param name="custom_joints_velocity_limits_names">( )</param>
+            <!-- the upper bound is "+", while the lower bounds are "-" -->
+            <!--param name="custom_joints_velocity_limits_values">(10.0, 15.0)</param-->
+            <param name="custom_joints_velocity_limits_values">( )</param>
+        <!-- **** base velocity limit: x, y, z, roll, pitch, yaw ****-->
+            <param name="base_velocity_limit_upper_buond">(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)</param>
+            <param name="base_velocity_limit_lower_buond">(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)</param>
+        <!-- Custom joint Configuration constraints-->
+        <!-- if the boudary value is inf, I will use -1000.0 rad, or +1000.0 rad-->
+            <param name="custom_constraint_variables">(
+             l_shoulder_roll, r_shoulder_roll)</param>
+            <param name="custom_constraint_matrix"> (
+            (1.0, 0.0),
+            (0.0, 1.0))</param>
+            <param name="custom_constraint_lower_bound"> (
+            -100.0, -100.0)</param>
+            <param name="custom_constraint_upper_bound"> (
+             1.4, 1.4)</param>
+            <param name="k_u">0.5</param>
+            <param name="k_l">0.5</param>
+        </group>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <elem name="HumanStateProviderLabel">XSenseIWearRemapper</elem>
+            </paramlist>
+        </action>
+        <action phase="shutdown" level="5" type="detach"/>
+    </device>
+
+    <!-- Uncomment to stream the output of HumanStateProvider on a YARP port -->
+    <device type="human_state_wrapper" name="RobotStateWrapper">
+        <param name="period">0.01</param>
+        <param name="outputPort">/ergoCub/RobotStateWrapper/state:o</param>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <elem name="HumanStateWrapperLabel">RobotStateProvider</elem>
+            </paramlist>
+        </action>
+        <action phase="shutdown" level="5" type="detach"/>
+    </device>
+
+    <device type="wearable_targets_wrapper" name="WearableTargetsWrapper">
+        <param name="period">0.01</param>
+        <param name="outputPort">/HDE/WearableTargetsWrapper/state:o</param>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <elem name="HumanStateWrapperLabel">RobotStateProvider</elem>
+            </paramlist>
+        </action>
+        <action phase="shutdown" level="5" type="detach"/>
+    </device>
+
+</robot>