Skip to content

Commit

Permalink
Allow RobotState::setFromIK to work with subframes (#3077)
Browse files Browse the repository at this point in the history 
* 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 <[email protected]>
Co-authored-by: Sebastian Jahr <[email protected]>
  • Loading branch information
@TSNoble @rr-tom-noble @sjahr
3 people authored Nov 13, 2024
1 parent 2490e51 commit ab34495
Show file tree
Hide file tree
Showing 2 changed files with 200 additions and 29 deletions.
50 changes: 21 additions & 29 deletions moveit_core/robot_state/src/robot_state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<const LinkModel* const, Eigen::Isometry3d>& 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)
Expand Down
179 changes: 179 additions & 0 deletions ...ners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>& joints1, const std::vector<double>& 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<double> 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_;
Expand Down Expand Up @@ -167,6 +196,43 @@ bool TrajectoryFunctionsTestBase::tfNear(const Eigen::Isometry3d& pose1, const E
return true;
}

bool TrajectoryFunctionsTestBase::jointsNear(const std::vector<double>& joints1, const std::vector<double>& 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<double> TrajectoryFunctionsTestBase::getJoints(const moveit::core::JointModelGroup* jmg,
const moveit::core::RobotState& state)
{
std::vector<double> 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<moveit::core::AttachedBody>(
link, object_name, object_pose, std::vector<shapes::ShapeConstPtr>{}, EigenSTL::vector_Isometry3d{},
std::set<std::string>{}, trajectory_msgs::msg::JointTrajectory{}, subframes));
}

/**
* @brief Parametrized class for tests with and without gripper.
*/
Expand Down Expand Up @@ -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<double> 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<double> 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<double> 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<double> 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<double> 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<double> 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<double> 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<double> 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
Expand Down

0 comments on commit ab34495

Please sign in to comment.