added an orientation task for each node

ami-iit · Dec 11, 2023 · 1feff13 · 1feff13
1 parent 141b569
commit 1feff13
Show file tree

Hide file tree

Showing 3 changed files with 165 additions and 11 deletions.
diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
@@ -36,7 +36,16 @@ class HumanIK
     Eigen::Vector3d m_baseAngularVelocity;
 
     // tasks
-    std::shared_ptr<BipedalLocomotion::IK::SO3Task> m_link1OrientationTask;
+    std::shared_ptr<BipedalLocomotion::IK::SO3Task> m_PelvisTask;
+    std::shared_ptr<BipedalLocomotion::IK::SO3Task> m_T8Task;
+    std::shared_ptr<BipedalLocomotion::IK::SO3Task> m_RightUpperArmTask;
+    std::shared_ptr<BipedalLocomotion::IK::SO3Task> m_RightForeArmTask;
+    std::shared_ptr<BipedalLocomotion::IK::SO3Task> m_LeftUpperArmTask;
+    std::shared_ptr<BipedalLocomotion::IK::SO3Task> m_LeftForeArmTask;
+    std::shared_ptr<BipedalLocomotion::IK::SO3Task> m_RightUpperLegTask;
+    std::shared_ptr<BipedalLocomotion::IK::SO3Task> m_RightLowerLegTask;
+    std::shared_ptr<BipedalLocomotion::IK::SO3Task> m_LeftUpperLegTask;
+    std::shared_ptr<BipedalLocomotion::IK::SO3Task> m_LeftLowerLegTask;
 
     // Number of Joint Degrees of Freedom
     int m_nrDoFs;
@@ -67,8 +76,36 @@ class HumanIK
     // set the initial joint positions
     bool setInitialJointPositions(const Eigen::Ref<const Eigen::VectorXd> qInitial);
 
-    bool setLink1OrientationAndAngVel(const manif::SO3d &link1Orientation,
-                                      const manif::SO3Tangentd &link1AngularVelocity);
+    // set the set point for the orientation tasks
+    bool setPelvisSetPoint(const manif::SO3d &pelvisOrientation,
+                                      const manif::SO3Tangentd &pelvisAngularVelocity);
+
+    bool setT8SetPoint(const manif::SO3d &T8Orientation,
+                                      const manif::SO3Tangentd &T8AngularVelocity);
+
+    bool setRightUpperArmSetPoint(const manif::SO3d &RightUpperArmOrientation,
+                                        const manif::SO3Tangentd &RightUpperArmAngularVelocity);
+
+    bool setRightForeArmSetPoint(const manif::SO3d &RightForeArmOrientation,
+                                        const manif::SO3Tangentd &RightForeArmAngularVelocity);
+
+    bool setLeftUpperArmSetPoint(const manif::SO3d &LeftUpperArmOrientation,
+                                        const manif::SO3Tangentd &LeftUpperArmAngularVelocity);
+
+    bool setLeftForeArmSetPoint(const manif::SO3d &LeftForeArmOrientation,
+                                        const manif::SO3Tangentd &LeftForeArmAngularVelocity);
+
+    bool setRightUpperLegSetPoint(const manif::SO3d &RightUpperLegOrientation,
+                                        const manif::SO3Tangentd &RightUpperLegAngularVelocity);
+
+    bool setRightLowerLegSetPoint(const manif::SO3d &RightLowerLegOrientation,
+                                        const manif::SO3Tangentd &RightLowerLegAngularVelocity);
+
+    bool setLeftUpperLegSetPoint(const manif::SO3d &LeftUpperLegOrientation,
+                                        const manif::SO3Tangentd &LeftUpperLegAngularVelocity);
+
+    bool setLeftLowerLegSetPoint(const manif::SO3d &LeftLowerLegOrientation,
+                                        const manif::SO3Tangentd &LeftLowerLegAngularVelocity);
 
     // compute the next state
     bool advance();

diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp
@@ -28,10 +28,55 @@ bool HumanIK::initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandle
     group->getParameter("robot_velocity_variable_name", variable);
     m_variableHandler.addVariable(variable, kinDyn->getNrOfDegreesOfFreedom() + 6);
 
-    m_link1OrientationTask = std::make_shared<BipedalLocomotion::IK::SO3Task>();
-    ok = ok && m_link1OrientationTask->setKinDyn(kinDyn);
-    ok = ok && m_link1OrientationTask->initialize(ptr->getGroup("LINK1_TASK"));
-    ok = ok && m_qpIK.addTask(m_link1OrientationTask, "link1_task", highPriority);
+    m_PelvisTask = std::make_shared<BipedalLocomotion::IK::SO3Task>();
+    ok = ok && m_PelvisTask->setKinDyn(kinDyn);
+    ok = ok && m_PelvisTask->initialize(ptr->getGroup("PELVIS_TASK"));
+    ok = ok && m_qpIK.addTask(m_PelvisTask, "pelvis_task", highPriority);
+
+    m_T8Task = std::make_shared<BipedalLocomotion::IK::SO3Task>();
+    ok = ok && m_T8Task->setKinDyn(kinDyn);
+    ok = ok && m_T8Task->initialize(ptr->getGroup("T8_TASK"));
+    ok = ok && m_qpIK.addTask(m_T8Task, "t8_task", highPriority);
+
+    m_RightUpperArmTask = std::make_shared<BipedalLocomotion::IK::SO3Task>();
+    ok = ok && m_RightUpperArmTask->setKinDyn(kinDyn);
+    ok = ok && m_RightUpperArmTask->initialize(ptr->getGroup("RIGHT_UPPER_ARM_TASK"));
+    ok = ok && m_qpIK.addTask(m_RightUpperArmTask, "right_upper_arm_task", lowPriority);
+
+    m_RightForeArmTask = std::make_shared<BipedalLocomotion::IK::SO3Task>();
+    ok = ok && m_RightForeArmTask->setKinDyn(kinDyn);
+    ok = ok && m_RightForeArmTask->initialize(ptr->getGroup("RIGHT_FORE_ARM_TASK"));
+    ok = ok && m_qpIK.addTask(m_RightForeArmTask, "right_fore_arm_task", lowPriority);
+
+    m_LeftUpperArmTask = std::make_shared<BipedalLocomotion::IK::SO3Task>();
+    ok = ok && m_LeftUpperArmTask->setKinDyn(kinDyn);
+    ok = ok && m_LeftUpperArmTask->initialize(ptr->getGroup("LEFT_UPPER_ARM_TASK"));
+    ok = ok && m_qpIK.addTask(m_LeftUpperArmTask, "left_upper_arm_task", lowPriority);
+
+    m_LeftForeArmTask = std::make_shared<BipedalLocomotion::IK::SO3Task>();
+    ok = ok && m_LeftForeArmTask->setKinDyn(kinDyn);
+    ok = ok && m_LeftForeArmTask->initialize(ptr->getGroup("LEFT_FORE_ARM_TASK"));
+    ok = ok && m_qpIK.addTask(m_LeftForeArmTask, "left_fore_arm_task", lowPriority);
+
+    m_RightUpperLegTask = std::make_shared<BipedalLocomotion::IK::SO3Task>();
+    ok = ok && m_RightUpperLegTask->setKinDyn(kinDyn);
+    ok = ok && m_RightUpperLegTask->initialize(ptr->getGroup("RIGHT_UPPER_LEG_TASK"));
+    ok = ok && m_qpIK.addTask(m_RightUpperLegTask, "right_upper_leg_task", lowPriority);
+
+    m_RightLowerLegTask = std::make_shared<BipedalLocomotion::IK::SO3Task>();
+    ok = ok && m_RightLowerLegTask->setKinDyn(kinDyn);
+    ok = ok && m_RightLowerLegTask->initialize(ptr->getGroup("RIGHT_LOWER_LEG_TASK"));
+    ok = ok && m_qpIK.addTask(m_RightLowerLegTask, "right_lower_leg_task", lowPriority);
+
+    m_LeftUpperLegTask = std::make_shared<BipedalLocomotion::IK::SO3Task>();
+    ok = ok && m_LeftUpperLegTask->setKinDyn(kinDyn);
+    ok = ok && m_LeftUpperLegTask->initialize(ptr->getGroup("LEFT_UPPER_LEG_TASK"));
+    ok = ok && m_qpIK.addTask(m_LeftUpperLegTask, "left_upper_leg_task", lowPriority);
+
+    m_LeftLowerLegTask = std::make_shared<BipedalLocomotion::IK::SO3Task>();
+    ok = ok && m_LeftLowerLegTask->setKinDyn(kinDyn);
+    ok = ok && m_LeftLowerLegTask->initialize(ptr->getGroup("LEFT_LOWER_LEG_TASK"));
+    ok = ok && m_qpIK.addTask(m_LeftLowerLegTask, "left_lower_leg_task", lowPriority);
 
     m_qpIK.finalize(m_variableHandler);
 
@@ -71,10 +116,82 @@ bool HumanIK::setInitialJointPositions(const Eigen::Ref<const Eigen::VectorXd> q
     return true;
 }
 
-bool HumanIK::setLink1OrientationAndAngVel(const manif::SO3d &link1Orientation,
-                                           const manif::SO3Tangentd &link1AngularVelocity)
+bool HumanIK::setPelvisSetPoint(const manif::SO3d &pelvisOrientation,
+                                           const manif::SO3Tangentd &pelvisAngularVelocity)
 {
-    m_link1OrientationTask->setSetPoint(link1Orientation, link1AngularVelocity);
+    m_PelvisTask->setSetPoint(pelvisOrientation, pelvisAngularVelocity);
+
+    return true;
+}
+
+bool HumanIK::setT8SetPoint(const manif::SO3d &T8Orientation,
+                                        const manif::SO3Tangentd &T8AngularVelocity)
+{
+    m_T8Task->setSetPoint(T8Orientation, T8AngularVelocity);
+
+    return true;
+}
+
+bool HumanIK::setRightUpperArmSetPoint(const manif::SO3d &rightUpperArmOrientation,
+                                        const manif::SO3Tangentd &rightUpperArmAngularVelocity)
+{
+    m_RightUpperArmTask->setSetPoint(rightUpperArmOrientation, rightUpperArmAngularVelocity);
+
+    return true;
+}
+
+bool HumanIK::setRightForeArmSetPoint(const manif::SO3d &rightForeArmOrientation,
+                                        const manif::SO3Tangentd &rightForeArmAngularVelocity)
+{
+    m_RightForeArmTask->setSetPoint(rightForeArmOrientation, rightForeArmAngularVelocity);
+
+    return true;
+}
+
+bool HumanIK::setLeftUpperArmSetPoint(const manif::SO3d &leftUpperArmOrientation,
+                                        const manif::SO3Tangentd &leftUpperArmAngularVelocity)
+{
+    m_LeftUpperArmTask->setSetPoint(leftUpperArmOrientation, leftUpperArmAngularVelocity);
+
+    return true;
+}
+
+bool HumanIK::setLeftForeArmSetPoint(const manif::SO3d &leftForeArmOrientation,
+                                        const manif::SO3Tangentd &leftForeArmAngularVelocity)
+{
+    m_LeftForeArmTask->setSetPoint(leftForeArmOrientation, leftForeArmAngularVelocity);
+
+    return true;
+}
+
+bool HumanIK::setRightUpperLegSetPoint(const manif::SO3d &rightUpperLegOrientation,
+                                        const manif::SO3Tangentd &rightUpperLegAngularVelocity)
+{
+    m_RightUpperLegTask->setSetPoint(rightUpperLegOrientation, rightUpperLegAngularVelocity);
+
+    return true;
+}
+
+bool HumanIK::setRightLowerLegSetPoint(const manif::SO3d &rightLowerLegOrientation,
+                                        const manif::SO3Tangentd &rightLowerLegAngularVelocity)
+{
+    m_RightLowerLegTask->setSetPoint(rightLowerLegOrientation, rightLowerLegAngularVelocity);
+
+    return true;
+}
+
+bool HumanIK::setLeftUpperLegSetPoint(const manif::SO3d &leftUpperLegOrientation,
+                                        const manif::SO3Tangentd &leftUpperLegAngularVelocity)
+{
+    m_LeftUpperLegTask->setSetPoint(leftUpperLegOrientation, leftUpperLegAngularVelocity);
+
+    return true;
+}
+
+bool HumanIK::setLeftLowerLegSetPoint(const manif::SO3d &leftLowerLegOrientation,
+                                        const manif::SO3Tangentd &leftLowerLegAngularVelocity)
+{
+    m_LeftLowerLegTask->setSetPoint(leftLowerLegOrientation, leftLowerLegAngularVelocity);
 
     return true;
 }

diff --git a/src/IK/tests/HumanIKTest.cpp b/src/IK/tests/HumanIKTest.cpp
@@ -55,7 +55,7 @@ TEST_CASE("InverseKinematic test")
 
     ik.setDt(0.1);
     ik.setInitialJointPositions(qInitial);
-    ik.setLink1OrientationAndAngVel(orientationDesired, angVelDesired);
+    ik.setPelvisSetPoint(orientationDesired, angVelDesired);
     ik.advance();
     ik.getJointPositions(JointPositions);
     ik.getJointVelocities(JointVelocities);