diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h index 7adb730..945d314 100644 --- 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 m_link1OrientationTask; + std::shared_ptr m_PelvisTask; + std::shared_ptr m_T8Task; + std::shared_ptr m_RightUpperArmTask; + std::shared_ptr m_RightForeArmTask; + std::shared_ptr m_LeftUpperArmTask; + std::shared_ptr m_LeftForeArmTask; + std::shared_ptr m_RightUpperLegTask; + std::shared_ptr m_RightLowerLegTask; + std::shared_ptr m_LeftUpperLegTask; + std::shared_ptr 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 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 index 9e0e91d..7a87c4b 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -28,10 +28,55 @@ bool HumanIK::initialize(std::weak_ptrgetParameter("robot_velocity_variable_name", variable); m_variableHandler.addVariable(variable, kinDyn->getNrOfDegreesOfFreedom() + 6); - m_link1OrientationTask = std::make_shared(); - 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(); + 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(); + 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(); + 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(); + 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(); + 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(); + 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(); + 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(); + 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(); + 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(); + 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 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 index 4de4a52..d4522a4 100644 --- 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);