From 5049e4dd733e07d062488261d27a3da090c92698 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 8 Nov 2024 15:27:44 +0100 Subject: [PATCH 1/4] Fix jacobian calculation (#3069) * Fix faulty Jacobian * Update tests * Format * More verbose error message Signed-off-by: Sebastian Jahr * Fix columns --------- Signed-off-by: Sebastian Jahr --- moveit_core/robot_state/src/robot_state.cpp | 17 ++- .../robot_state/test/robot_state_test.cpp | 109 +++++++++++++++++- 2 files changed, 115 insertions(+), 11 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 95714bc7e2..9f36336dda 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1496,19 +1496,25 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link const int rows = use_quaternion_representation ? 7 : 6; const int columns = group->getVariableCount(); jacobian.resize(rows, columns); + jacobian.setZero(); // Get the tip pose with respect to the group root link. Append the user-requested offset 'reference_point_position'. const Eigen::Isometry3d root_pose_tip = root_pose_world * getGlobalLinkTransform(link); const Eigen::Vector3d tip_point = root_pose_tip * reference_point_position; - // Here we iterate over all the group active joints, and compute how much each of them contribute to the Cartesian - // displacement at the tip. So we build the Jacobian incrementally joint by joint. - std::size_t active_joints = group->getActiveJointModels().size(); + // Initialize the column index of the Jacobian matrix. int i = 0; - for (std::size_t joint = 0; joint < active_joints; ++joint) + + // Here we iterate over all the group active joints, and compute how much each of them contribute to the Cartesian + // displacement at the tip. So we build the Jacobian incrementally joint by joint up to the parent joint of the reference link. + for (const JointModel* joint_model : joint_models) { + // Stop looping if we reached the child joint of the reference link. + if (joint_model->getParentLinkModel() == link) + { + break; + } // Get the child link for the current joint, and its pose with respect to the group root link. - const JointModel* joint_model = joint_models[joint]; const LinkModel* child_link_model = joint_model->getChildLinkModel(); const Eigen::Isometry3d& root_pose_link = root_pose_world * getGlobalLinkTransform(child_link_model); @@ -1540,6 +1546,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link RCLCPP_ERROR(getLogger(), "Unknown type of joint in Jacobian computation"); return false; } + i += joint_model->getVariableCount(); } diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index cafb5bf01f..bea2b7e229 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -61,7 +61,8 @@ Eigen::VectorXd makeVector(const std::vector& values) // Checks the validity of state.getJacobian() at the given 'joint_values' and 'joint_velocities'. void checkJacobian(moveit::core::RobotState& state, const moveit::core::JointModelGroup& joint_model_group, - const Eigen::VectorXd& joint_values, const Eigen::VectorXd& joint_velocities) + const Eigen::VectorXd& joint_values, const Eigen::VectorXd& joint_velocities, + const moveit::core::LinkModel* reference_link = nullptr) { // Using the Jacobian, compute the Cartesian velocity vector at which the end-effector would move, with the given // joint velocities. @@ -73,9 +74,36 @@ void checkJacobian(moveit::core::RobotState& state, const moveit::core::JointMod const moveit::core::LinkModel* root_link_model = root_joint_model->getParentLinkModel(); const Eigen::Isometry3d root_pose_world = state.getGlobalLinkTransform(root_link_model).inverse(); - const Eigen::Isometry3d tip_pose_initial = - root_pose_world * state.getGlobalLinkTransform(joint_model_group.getLinkModels().back()); - const Eigen::MatrixXd jacobian = state.getJacobian(&joint_model_group); + if (!reference_link) + { + reference_link = joint_model_group.getLinkModels().back(); + } + const Eigen::Isometry3d tip_pose_initial = root_pose_world * state.getGlobalLinkTransform(reference_link); + Eigen::MatrixXd jacobian; + state.getJacobian(&joint_model_group, reference_link, Eigen::Vector3d::Zero(), jacobian); + + // Verify that only elements of the Jacobian contain values that correspond to joints that are being used based on the reference link. + const std::vector& joint_models = joint_model_group.getJointModels(); + auto it = std::find_if(joint_models.begin(), joint_models.end(), [&](const moveit::core::JointModel* jm) { + return jm->getParentLinkModel() == reference_link; + }); + if (it != joint_models.end()) + { + std::size_t index = 0; + for (auto jt = joint_models.begin(); jt != it; ++jt) + { + index += (*jt)->getVariableCount(); + } + + EXPECT_TRUE(jacobian.block(0, index, jacobian.rows(), jacobian.cols() - index).isZero()) + << "Jacobian contains non-zero values for joints that are not used based on the reference link " + << reference_link->getName() << ". This is the faulty Jacobian: " << '\n' + << jacobian << '\n' + << "The columns " << index << " to " << jacobian.cols() << " should be zero. Instead the values are: " << '\n' + << jacobian.block(0, index, jacobian.rows(), jacobian.cols() - index); + } + + // Compute the Cartesian velocity vector using the Jacobian. const Eigen::VectorXd cartesian_velocity = jacobian * joint_velocities; // Compute the instantaneous displacement that the end-effector would achieve if the given joint @@ -84,8 +112,7 @@ void checkJacobian(moveit::core::RobotState& state, const moveit::core::JointMod const Eigen::VectorXd delta_joint_angles = time_step * joint_velocities; state.setJointGroupPositions(&joint_model_group, joint_values + delta_joint_angles); state.updateLinkTransforms(); - const Eigen::Isometry3d tip_pose_after_delta = - root_pose_world * state.getGlobalLinkTransform(joint_model_group.getLinkModels().back()); + const Eigen::Isometry3d tip_pose_after_delta = root_pose_world * state.getGlobalLinkTransform(reference_link); const Eigen::Vector3d displacement = tip_pose_after_delta.translation() - tip_pose_initial.translation(); // The Cartesian velocity vector obtained via the Jacobian should be aligned with the instantaneous robot motion, i.e. @@ -843,6 +870,76 @@ TEST(getJacobian, RevoluteJoints) checkJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 })); } +TEST(getJacobian, RevoluteJointsButDifferentLink) +{ + // Robot URDF with four revolute joints. + constexpr char robot_urdf[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + constexpr char robot_srdf[] = R"xml( + + + + + + + + + + )xml"; + + const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf); + ASSERT_TRUE(urdf_model); + const auto srdf_model = std::make_shared(); + ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf)); + const auto robot_model = std::make_shared(urdf_model, srdf_model); + + moveit::core::RobotState state(robot_model); + const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip"); + + // Some made-up numbers, at zero and non-zero robot configurations. + checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 }), + state.getLinkModel("link_c")); + checkJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 }), + state.getLinkModel("link_c")); +} + TEST(getJacobian, RevoluteAndPrismaticJoints) { // Robot URDF with three revolute joints and one prismatic joint. From 4c28a74343e1d8470689cae9e23eb30c91bdcb4d Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 18:11:36 +0000 Subject: [PATCH 2/4] Enhancement/ports moveit 3522 (#3070) * Makes PTP changes * Makes LIN changes * Makes CIRC changes * Makes CIRC changes * Makes CIRC changes * Makes CIRC changes * Makes LIN changes * Makes LIN changes * Fixes formatting * Replace ROS_WARN with RCLCPP_WARN * Fixes formatting * Fixes RCLCPP_WARN calls * Fixes compiler errors * Fixes if statement * Fixes formatting * Fixes msg type --- .../src/trajectory_generator_circ.cpp | 64 ++++++++++++------- .../src/trajectory_generator_lin.cpp | 30 +++++---- .../src/trajectory_generator_ptp.cpp | 29 +++++++-- 3 files changed, 85 insertions(+), 38 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index ae1e7b9b10..e0ef8ce126 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -97,7 +97,6 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request."); info.group_name = req.group_name; - std::string frame_id{ robot_model_->getModelFrame() }; moveit::core::RobotState robot_state = scene->getCurrentState(); // goal given in joint space @@ -130,6 +129,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni // goal given in Cartesian space else { + std::string frame_id; info.link_name = req.goal_constraints.front().position_constraints.front().link_name; if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) @@ -142,39 +142,59 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni { frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; } - info.goal_pose = getConstraintPose(req.goal_constraints.front()); + + // goal pose with optional offset wrt. the planning frame + info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front()); + frame_id = robot_model_->getModelFrame(); + + // check goal pose ik before Cartesian motion plan starts + std::map ik_solution; + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + ik_solution)) + { + // LCOV_EXCL_START + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw CircInverseForGoalIncalculable(os.str()); + // LCOV_EXCL_STOP // not able to trigger here since lots of checks before + // are in place + } } computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); - // check goal pose ik before Cartesian motion plan starts - std::map ik_solution; - if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, - ik_solution)) + // center point with wrt. the planning frame + std::string center_point_frame_id; + + info.circ_path_point.first = req.path_constraints.name; + if (req.path_constraints.position_constraints.front().header.frame_id.empty()) { - // LCOV_EXCL_START - std::ostringstream os; - os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; - throw CircInverseForGoalIncalculable(os.str()); - // LCOV_EXCL_STOP // not able to trigger here since lots of checks before - // are in place + RCLCPP_WARN(getLogger(), "Frame id is not set in position constraints of " + "path. Use model frame as default"); + center_point_frame_id = robot_model_->getModelFrame(); } - info.circ_path_point.first = req.path_constraints.name; + else + { + center_point_frame_id = req.path_constraints.position_constraints.front().header.frame_id; + } + + Eigen::Isometry3d center_point_pose; + tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front(), + center_point_pose); + + center_point_pose = scene->getFrameTransform(center_point_frame_id) * center_point_pose; + if (!req.goal_constraints.front().position_constraints.empty()) { const moveit_msgs::msg::Constraints& goal = req.goal_constraints.front(); - info.circ_path_point.second = - getConstraintPose( - req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, - goal.orientation_constraints.front().orientation, goal.position_constraints.front().target_point_offset) - .translation(); + geometry_msgs::msg::Point center_point = tf2::toMsg(Eigen::Vector3d(center_point_pose.translation())); + info.circ_path_point.second = getConstraintPose(center_point, goal.orientation_constraints.front().orientation, + goal.position_constraints.front().target_point_offset) + .translation(); } else { - Eigen::Vector3d circ_path_point; - tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, - circ_path_point); - info.circ_path_point.second = circ_path_point; + info.circ_path_point.second = center_point_pose.translation(); } } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 43e5235e0c..941290b90e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -74,7 +74,6 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request."); info.group_name = req.group_name; - std::string frame_id{ robot_model_->getModelFrame() }; moveit::core::RobotState robot_state = scene->getCurrentState(); // goal given in joint space @@ -103,6 +102,8 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin // goal given in Cartesian space else { + std::string frame_id; + info.link_name = req.goal_constraints.front().position_constraints.front().link_name; if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) @@ -115,20 +116,25 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin { frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; } - info.goal_pose = getConstraintPose(req.goal_constraints.front()); - } - computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); + // goal pose with optional offset wrt. the planning frame + info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front()); + frame_id = robot_model_->getModelFrame(); - // check goal pose ik before Cartesian motion plan starts - std::map ik_solution; - if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, - ik_solution)) - { - std::ostringstream os; - os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; - throw LinInverseForGoalIncalculable(os.str()); + // check goal pose ik before Cartesian motion plan starts + std::map ik_solution; + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + ik_solution)) + { + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw LinInverseForGoalIncalculable(os.str()); + } } + + // Ignored return value because at this point the function should always + // return 'true'. + computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); } void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index b12f64224c..f7c45ac29a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -233,11 +233,32 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin // solve the ik else { - Eigen::Isometry3d goal_pose = getConstraintPose(req.goal_constraints.front()); - if (!computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name, - goal_pose, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position)) + std::string frame_id; + + info.link_name = req.goal_constraints.front().position_constraints.front().link_name; + if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || + req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) + { + RCLCPP_WARN(getLogger(), "Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); + frame_id = robot_model_->getModelFrame(); + } + else + { + frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; + } + + // goal pose with optional offset wrt. the planning frame + info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front()); + frame_id = robot_model_->getModelFrame(); + + // check goal pose ik before Cartesian motion plan start + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + info.goal_joint_position)) { - throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose"); + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw PtpNoIkSolutionForGoalPose(os.str()); } } } From 2490e51444df20bf64c05deacf903785703852b7 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Mon, 11 Nov 2024 09:38:24 -0500 Subject: [PATCH 3/4] Remove Humble and Iron from tutorial Docker CI job (#3081) --- .github/workflows/tutorial_docker.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/tutorial_docker.yaml b/.github/workflows/tutorial_docker.yaml index cd305eec20..72c2b80252 100644 --- a/.github/workflows/tutorial_docker.yaml +++ b/.github/workflows/tutorial_docker.yaml @@ -16,7 +16,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling, humble, iron, jazzy] + ROS_DISTRO: [rolling, jazzy] runs-on: ubuntu-latest permissions: packages: write From ab34495d2d901fb9f053b706bb610c574b5bc0d6 Mon Sep 17 00:00:00 2001 From: Tom Noble <53005340+TSNoble@users.noreply.github.com> Date: Wed, 13 Nov 2024 11:43:12 +0000 Subject: [PATCH 4/4] Allow RobotState::setFromIK to work with subframes (#3077) * Adds regression tests for setFromIK with objects. Adds failing tests demonstrating failure with subframes * Modifies RobotState::setFromIK to account for subframes * Fixes formatting * Fixes formatting * Fixes formatting * Applies PR suggestions * Applies PR comments --------- Co-authored-by: Tom Noble Co-authored-by: Sebastian Jahr --- moveit_core/robot_state/src/robot_state.cpp | 50 ++--- .../src/unittest_trajectory_functions.cpp | 179 ++++++++++++++++++ 2 files changed, 200 insertions(+), 29 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 9f36336dda..1f1593ca78 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1877,42 +1877,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(getLogger(), "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 LinkModel* link_model = getLinkModel(pose_frame); - if (!link_model) - { - RCLCPP_ERROR(getLogger(), "The following Pose Frame does not exist: %s", pose_frame.c_str()); - return false; - } - const 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(getLogger(), "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_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 b13b1c5606..a0383aaa24 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, 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_; @@ -167,6 +196,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. */ @@ -338,6 +404,119 @@ 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 no subframes, and no transform + Eigen::Isometry3d object_pose_in_tip = Eigen::Isometry3d::Identity(); + attachToLink(state, tip_link, "object", object_pose_in_tip, { {} }); + + // 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()); + + attachToLink(state, tip_link, "object", object_pose_in_tip, { {} }); + + // 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