inline EigenHelpers::VectorIsometry3d GetLinkTransforms(const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity()) { assert(configuration.size() == UR_10_NUM_ACTIVE_JOINTS); EigenHelpers::VectorIsometry3d link_transforms(UR_10_NUM_LINKS); link_transforms[0] = base_transform; link_transforms[1] = link_transforms[0] * Get_link_0_joint_1_LinkJointTransform(configuration[0]); link_transforms[2] = link_transforms[1] * Get_link_1_joint_2_LinkJointTransform(configuration[1]); link_transforms[3] = link_transforms[2] * Get_link_2_joint_3_LinkJointTransform(configuration[2]); link_transforms[4] = link_transforms[3] * Get_link_3_joint_4_LinkJointTransform(configuration[3]); link_transforms[5] = link_transforms[4] * Get_link_4_joint_5_LinkJointTransform(configuration[4]); link_transforms[6] = link_transforms[5] * Get_link_5_joint_6_LinkJointTransform(configuration[5]); return link_transforms; }
inline VectorAffine3d GetLinkTransforms(const std::vector<double>& configuration) { assert(configuration.size() == ABB_IRB1600_145_NUM_ACTIVE_JOINTS); VectorAffine3d link_transforms(ABB_IRB1600_145_NUM_LINKS); link_transforms[0] = Eigen::Affine3d::Identity(); link_transforms[1] = link_transforms[0] * Get_base_joint1_LinkJointTransform(configuration[0]); link_transforms[2] = link_transforms[1] * Get_link_1_joint_2_LinkJointTransform(configuration[1]); link_transforms[3] = link_transforms[2] * Get_link_2_joint_3_LinkJointTransform(configuration[2]); link_transforms[4] = link_transforms[3] * Get_link_3_joint_4_LinkJointTransform(configuration[3]); link_transforms[5] = link_transforms[4] * Get_link_4_joint_5_LinkJointTransform(configuration[4]); link_transforms[6] = link_transforms[5] * Get_link_5_joint_6_LinkJointTransform(configuration[5]); link_transforms[7] = link_transforms[6] * Get_Fixed_link_6_joint_tool_LinkJointTransform(); return link_transforms; }