diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 0d96e1755c..78614304a0 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1668,40 +1668,34 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is if (pose_frame != solver_tip_frame) { - if (hasAttachedBody(pose_frame)) + auto* pose_parent = getRigidlyConnectedParentLinkModel(pose_frame); + if (!pose_parent) { - const AttachedBody* body = getAttachedBody(pose_frame); - pose_frame = body->getAttachedLinkName(); - pose = pose * body->getPose().inverse(); + RCLCPP_ERROR_STREAM(LOGGER, "The following Pose Frame does not exist: " << pose_frame); + return false; } - if (pose_frame != solver_tip_frame) + Eigen::Isometry3d pose_parent_to_frame = getFrameTransform(pose_frame); + auto* tip_parent = getRigidlyConnectedParentLinkModel(solver_tip_frame); + if (!tip_parent) { - const moveit::core::LinkModel* link_model = getLinkModel(pose_frame); - if (!link_model) - { - RCLCPP_ERROR(LOGGER, "The following Pose Frame does not exist: %s", pose_frame.c_str()); - return false; - } - const moveit::core::LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); - for (const std::pair& fixed_link : fixed_links) - if (Transforms::sameFrame(fixed_link.first->getName(), solver_tip_frame)) - { - pose_frame = solver_tip_frame; - pose = pose * fixed_link.second; - break; - } + RCLCPP_ERROR_STREAM(LOGGER, "The following Solver Tip Frame does not exist: " << solver_tip_frame); + return false; } - - } // end if pose_frame - - // Check if this pose frame works - if (pose_frame == solver_tip_frame) + Eigen::Isometry3d tip_parent_to_tip = getFrameTransform(solver_tip_frame); + if (pose_parent == tip_parent) + { + // transform goal pose as target for solver_tip_frame (instead of pose_frame) + pose = pose * pose_parent_to_frame.inverse() * tip_parent_to_tip; + found_valid_frame = true; + break; + } + } + else { found_valid_frame = true; break; - } - - } // end for solver_tip_frames + } // end if pose_frame + } // end for solver_tip_frames // Make sure one of the tip frames worked if (!found_valid_frame) diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 8d8fe4ce81..cdc49bbf62 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -691,6 +691,7 @@ TEST_F(OneRobot, rigidlyConnectedParent) EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a); moveit::core::RobotState state(robot_model_); + state.setToDefaultValues(); state.updateLinkTransforms(); EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp index c430e4173d..3a389fae1a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp @@ -135,6 +135,35 @@ class TrajectoryFunctionsTestBase : public testing::Test */ bool tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, const double& epsilon); + /** + * @brief check if two sets of joint positions are close + * @param joints1 the first set of joint positions to compare + * @param joints2 the second set of joint positions to compare + * @param epsilon the tolerance a all joint position diffs must satisfy + * @return false if any joint diff exceeds tolerance. true otherwise + */ + bool jointsNear(const std::vector& joints1, const std::vector& joints2, double epsilon); + + /** + * @brief get the current joint values of the robot state + * @param jmg the joint model group whose joints we are interested in + * @param state the robot state to fetch the current joint positions for + * @return the joint positions for joints from jmg, set to the positions determined from state + */ + std::vector getJoints(const moveit::core::JointModelGroup* jmg, const moveit::core::RobotState& state); + + /** + * @brief attach a collision object and subframes to a link + * @param state the state we are updating + * @param link the link we are attaching the collision object to + * @param object_name a unique name for the collision object + * @param object_pose the pose of the object relative to the parent link + * @param subframes subframe names and poses relative to the object they attach to + */ + void attachToLink(moveit::core::RobotState& state, const moveit::core::LinkModel* link, + const std::string& object_name, const Eigen::Isometry3d& object_pose, + const moveit::core::FixedTransformsMap& subframes); + protected: // ros stuff rclcpp::Node::SharedPtr node_; @@ -166,6 +195,43 @@ bool TrajectoryFunctionsTestBase::tfNear(const Eigen::Isometry3d& pose1, const E return true; } +bool TrajectoryFunctionsTestBase::jointsNear(const std::vector& joints1, const std::vector& joints2, + double epsilon) +{ + if (joints1.size() != joints2.size()) + { + return false; + } + for (std::size_t i = 0; i < joints1.size(); ++i) + { + if (fabs(joints1.at(i) - joints2.at(i)) > fabs(epsilon)) + { + return false; + } + } + return true; +} + +std::vector TrajectoryFunctionsTestBase::getJoints(const moveit::core::JointModelGroup* jmg, + const moveit::core::RobotState& state) +{ + std::vector joints; + for (const auto& name : jmg->getActiveJointModelNames()) + { + joints.push_back(state.getVariablePosition(name)); + } + return joints; +} + +void TrajectoryFunctionsTestBase::attachToLink(moveit::core::RobotState& state, const moveit::core::LinkModel* link, + const std::string& object_name, const Eigen::Isometry3d& object_pose, + const moveit::core::FixedTransformsMap& subframes) +{ + state.attachBody(std::make_unique( + link, object_name, object_pose, std::vector{}, EigenSTL::vector_Isometry3d{}, + std::set{}, trajectory_msgs::msg::JointTrajectory{}, subframes)); +} + /** * @brief Parametrized class for tests with and without gripper. */ @@ -329,6 +395,120 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState) } } +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithIdentityCollisionObject) +{ + // Set up a default robot + moveit::core::RobotState state(robot_model_); + state.setToDefaultValues(); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + std::vector default_joints = getJoints(jmg, state); + const moveit::core::LinkModel* tip_link = robot_model_->getLinkModel(tcp_link_); + Eigen::Isometry3d tip_pose_in_base = state.getFrameTransform(tcp_link_); + + // Attach an object with an ignored subframe, and no transform + Eigen::Isometry3d object_pose_in_tip = Eigen::Isometry3d::Identity(); + moveit::core::FixedTransformsMap subframes({ { "ignored", Eigen::Isometry3d::Identity() } }); + attachToLink(state, tip_link, "object", object_pose_in_tip, subframes); + + // The RobotState should be able to use an object pose to set the joints + Eigen::Isometry3d object_pose_in_base = tip_pose_in_base * object_pose_in_tip; + bool success = state.setFromIK(jmg, object_pose_in_base, "object"); + EXPECT_TRUE(success); + + // Given the target pose is the default pose of the object, the joints should be unchanged + std::vector ik_joints = getJoints(jmg, state); + EXPECT_TRUE(jointsNear(ik_joints, default_joints, 4 * IK_SEED_OFFSET)); +} + +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithTransformedCollisionObject) +{ + // Set up a default robot + moveit::core::RobotState state(robot_model_); + state.setToDefaultValues(); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + std::vector default_joints = getJoints(jmg, state); + const moveit::core::LinkModel* tip_link = robot_model_->getLinkModel(tcp_link_); + Eigen::Isometry3d tip_pose_in_base = state.getFrameTransform(tcp_link_); + + // Attach an object with no subframes, and a non-trivial transform + Eigen::Isometry3d object_pose_in_tip; + object_pose_in_tip = Eigen::Translation3d(1, 2, 3); + object_pose_in_tip *= Eigen::AngleAxis(M_PI_2, Eigen::Vector3d::UnitX()); + moveit::core::FixedTransformsMap subframes({ { "ignored", Eigen::Isometry3d::Identity() } }); + attachToLink(state, tip_link, "object", object_pose_in_tip, subframes); + + // The RobotState should be able to use an object pose to set the joints + Eigen::Isometry3d object_pose_in_base = tip_pose_in_base * object_pose_in_tip; + bool success = state.setFromIK(jmg, object_pose_in_base, "object"); + EXPECT_TRUE(success); + + // Given the target pose is the default pose of the object, the joints should be unchanged + std::vector ik_joints = getJoints(jmg, state); + EXPECT_TRUE(jointsNear(ik_joints, default_joints, 4 * IK_SEED_OFFSET)); +} + +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithIdentitySubframe) +{ + // Set up a default robot + moveit::core::RobotState state(robot_model_); + state.setToDefaultValues(); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + std::vector default_joints = getJoints(jmg, state); + const moveit::core::LinkModel* tip_link = robot_model_->getLinkModel(tcp_link_); + Eigen::Isometry3d tip_pose_in_base = state.getFrameTransform(tcp_link_); + + // Attach an object and subframe with no transforms + Eigen::Isometry3d object_pose_in_tip = Eigen::Isometry3d::Identity(); + Eigen::Isometry3d subframe_pose_in_object = Eigen::Isometry3d::Identity(); + moveit::core::FixedTransformsMap subframes({ { "subframe", subframe_pose_in_object } }); + attachToLink(state, tip_link, "object", object_pose_in_tip, subframes); + + // The RobotState should be able to use a subframe pose to set the joints + Eigen::Isometry3d subframe_pose_in_base = tip_pose_in_base * object_pose_in_tip * subframe_pose_in_object; + bool success = state.setFromIK(jmg, subframe_pose_in_base, "object/subframe"); + EXPECT_TRUE(success); + + // Given the target pose is the default pose of the subframe, the joints should be unchanged + std::vector ik_joints = getJoints(jmg, state); + EXPECT_TRUE(jointsNear(ik_joints, default_joints, 4 * IK_SEED_OFFSET)); +} + +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithTransformedSubframe) +{ + // Set up a default robot + moveit::core::RobotState state(robot_model_); + state.setToDefaultValues(); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + std::vector default_joints = getJoints(jmg, state); + const moveit::core::LinkModel* tip_link = robot_model_->getLinkModel(tcp_link_); + Eigen::Isometry3d tip_pose_in_base = state.getFrameTransform(tcp_link_); + + // Attach an object and subframe with non-trivial transforms + Eigen::Isometry3d object_pose_in_tip; + object_pose_in_tip = Eigen::Translation3d(1, 2, 3); + object_pose_in_tip *= Eigen::AngleAxis(M_PI_2, Eigen::Vector3d::UnitX()); + + Eigen::Isometry3d subframe_pose_in_object; + subframe_pose_in_object = Eigen::Translation3d(4, 5, 6); + subframe_pose_in_object *= Eigen::AngleAxis(M_PI_2, Eigen::Vector3d::UnitY()); + + moveit::core::FixedTransformsMap subframes({ { "subframe", subframe_pose_in_object } }); + attachToLink(state, tip_link, "object", object_pose_in_tip, subframes); + + // The RobotState should be able to use a subframe pose to set the joints + Eigen::Isometry3d subframe_pose_in_base = tip_pose_in_base * object_pose_in_tip * subframe_pose_in_object; + bool success = state.setFromIK(jmg, subframe_pose_in_base, "object/subframe"); + EXPECT_TRUE(success); + + // Given the target pose is the default pose of the subframe, the joints should be unchanged + std::vector ik_joints = getJoints(jmg, state); + EXPECT_TRUE(jointsNear(ik_joints, default_joints, 4 * IK_SEED_OFFSET)); +} + /** * @brief Test the wrapper function to compute inverse kinematics using robot * model