inline EigenHelpers::VectorIsometry3d GetLinkTransforms(const std::map<std::string, double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
{
std::vector<double> configuration_vector(UR_10_NUM_ACTIVE_JOINTS);
configuration_vector[0] = arc_helpers::RetrieveOrDefault(configuration, UR_10_ACTIVE_JOINT_1_NAME, 0.0);
configuration_vector[1] = arc_helpers::RetrieveOrDefault(configuration, UR_10_ACTIVE_JOINT_2_NAME, 0.0);
configuration_vector[2] = arc_helpers::RetrieveOrDefault(configuration, UR_10_ACTIVE_JOINT_3_NAME, 0.0);
configuration_vector[3] = arc_helpers::RetrieveOrDefault(configuration, UR_10_ACTIVE_JOINT_4_NAME, 0.0);
configuration_vector[4] = arc_helpers::RetrieveOrDefault(configuration, UR_10_ACTIVE_JOINT_5_NAME, 0.0);
configuration_vector[5] = arc_helpers::RetrieveOrDefault(configuration, UR_10_ACTIVE_JOINT_6_NAME, 0.0);
return GetLinkTransforms(configuration_vector, base_transform);
}
inline VectorAffine3d GetLinkTransforms(std::map<std::string, double> configuration)
{
std::vector<double> configuration_vector(ABB_IRB1600_145_NUM_ACTIVE_JOINTS);
configuration_vector[0] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_1_NAME];
configuration_vector[1] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_2_NAME];
configuration_vector[2] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_3_NAME];
configuration_vector[3] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_4_NAME];
configuration_vector[4] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_5_NAME];
configuration_vector[5] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_6_NAME];
return GetLinkTransforms(configuration_vector);
}
inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(const std::map<std::string, double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
{
const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
EigenHelpers::MapStringIsometry3d link_transforms_map;
link_transforms_map[UR_10_LINK_1_NAME] = link_transforms[0];
link_transforms_map[UR_10_LINK_2_NAME] = link_transforms[1];
link_transforms_map[UR_10_LINK_3_NAME] = link_transforms[2];
link_transforms_map[UR_10_LINK_4_NAME] = link_transforms[3];
link_transforms_map[UR_10_LINK_5_NAME] = link_transforms[4];
link_transforms_map[UR_10_LINK_6_NAME] = link_transforms[5];
link_transforms_map[UR_10_LINK_7_NAME] = link_transforms[6];
return link_transforms_map;
}