Skip to content

Commit

Permalink
Ports moveit/moveit/pull/3519 to ros2 (moveit#3055)
Browse files Browse the repository at this point in the history 
* Ports moveit/moveit/pull/3519 to ros2

* Applies formatting

* Fixes compilation errors

* Fixes compilation errors

* Fixes compilation errors

* Fixes compilation errors

* Replaces RobotState with pointer in tests

* Fixes tests
  • Loading branch information
@rr-tom-noble
rr-tom-noble authored Nov 5, 2024
1 parent b3b1f73 commit 70e1aae
Show file tree
Hide file tree
Showing 14 changed files with 85 additions and 179 deletions.
7 changes: 0 additions & 7 deletions ..._industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,13 +126,6 @@ void pilz_industrial_motion_planner::PlanningContextBase<GeneratorT>::solve(plan
res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
return;
}
// Use current state as start state if not set
if (request_.start_state.joint_state.name.empty())
{
moveit_msgs::msg::RobotState current_state;
moveit::core::robotStateToRobotStateMsg(getPlanningScene()->getCurrentState(), current_state);
request_.start_state = current_state;
}
generator_.generate(getPlanningScene(), request_, res);
}

Expand Down
14 changes: 7 additions & 7 deletions ...z_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@

#include <pilz_industrial_motion_planner/cartesian_trajectory.h>
#include <pilz_industrial_motion_planner/limits_container.h>
#include <pilz_industrial_motion_planner/trajectory_generation_exceptions.h>

namespace pilz_industrial_motion_planner
{
Expand Down Expand Up @@ -74,17 +75,17 @@ bool computePoseIK(const planning_scene::PlanningSceneConstPtr& scene, const std
bool check_self_collision = true, const double timeout = 0.0);

/**
* @brief compute the pose of a link at give robot state
* @param robot_model: kinematic model of the robot
* @brief compute the pose of a link at a given robot state
* @param robot_state: an arbitrary robot state (with collision objects attached)
* @param link_name: target link name
* @param joint_state: joint positions of this group
* @param pose: pose of the link in base frame of robot model
* @return true if succeed
*/
bool computeLinkFK(const planning_scene::PlanningSceneConstPtr& scene, const std::string& link_name,
bool computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name,
const std::map<std::string, double>& joint_state, Eigen::Isometry3d& pose);

bool computeLinkFK(const planning_scene::PlanningSceneConstPtr& scene, const std::string& link_name,
bool computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name,
const std::vector<std::string>& joint_names, const std::vector<double>& joint_positions,
Eigen::Isometry3d& pose);

Expand Down Expand Up @@ -212,9 +213,8 @@ bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p
* @param ik_solution
* @return
*/
bool isStateColliding(const bool test_for_self_collision, const planning_scene::PlanningSceneConstPtr& scene,
moveit::core::RobotState* state, const moveit::core::JointModelGroup* const group,
const double* const ik_solution);
bool isStateColliding(const planning_scene::PlanningSceneConstPtr& scene, moveit::core::RobotState* state,
const moveit::core::JointModelGroup* const group, const double* const ik_solution);
} // namespace pilz_industrial_motion_planner

void normalizeQuaternion(geometry_msgs::msg::Quaternion& quat);
Expand Down
17 changes: 9 additions & 8 deletions ...z_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,19 +111,21 @@ class TrajectoryGenerator

protected:
/**
* @brief This class is used to extract needed information from motion plan
* request.
* @brief This class is used to extract needed information from motion plan request.
*/
class MotionPlanInfo
{
public:
MotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req);

std::string group_name;
std::string link_name;
Eigen::Isometry3d start_pose;
Eigen::Isometry3d goal_pose;
std::map<std::string, double> start_joint_position;
std::map<std::string, double> goal_joint_position;
std::pair<std::string, Eigen::Vector3d> circ_path_point;
planning_scene::PlanningSceneConstPtr start_scene; // scene with updated start state
};

/**
Expand Down Expand Up @@ -199,7 +201,7 @@ class TrajectoryGenerator
* moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure
* @param req: motion plan request
*/
void validateRequest(const planning_interface::MotionPlanRequest& req) const;
void validateRequest(const planning_interface::MotionPlanRequest& req, const moveit::core::RobotState& rstate) const;

/**
* @brief set MotionPlanResponse from joint trajectory
Expand All @@ -226,14 +228,13 @@ class TrajectoryGenerator
void checkStartState(const moveit_msgs::msg::RobotState& start_state, const std::string& group) const;

void checkGoalConstraints(const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints,
const std::vector<std::string>& expected_joint_names, const std::string& group_name) const;
const std::string& group_name, const moveit::core::RobotState& rstate) const;

void checkJointGoalConstraint(const moveit_msgs::msg::Constraints& constraint,
const std::vector<std::string>& expected_joint_names,
const std::string& group_name) const;
void checkJointGoalConstraint(const moveit_msgs::msg::Constraints& constraint, const std::string& group_name) const;

void checkCartesianGoalConstraint(const moveit_msgs::msg::Constraints& constraint,
const std::string& group_name) const;
const moveit::core::RobotState& robot_state,
const moveit::core::JointModelGroup* const jmg) const;

private:
/**
Expand Down
2 changes: 0 additions & 2 deletions ...ustrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,8 +56,6 @@ CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownLinkNameOfAuxiliaryPoint,
moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME);
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NumberOfConstraintsMismatch,
moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircJointMissingInStartState,
moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);

/**
Expand Down
1 change: 0 additions & 1 deletion ...dustrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,6 @@ namespace pilz_industrial_motion_planner
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs::msg::MoveItErrorCodes::FAILURE);

CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinJointMissingInStartState, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);

/**
Expand Down
2 changes: 1 addition & 1 deletion moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend(

if (!generateJointTrajectory(planning_scene, limits_.getJointLimitContainer(), blend_trajectory_cartesian,
req.group_name, req.link_name, initial_joint_position, initial_joint_velocity,
blend_joint_trajectory, error_code, true))
blend_joint_trajectory, error_code))
{
// LCOV_EXCL_START
RCLCPP_INFO(getLogger(), "Failed to generate joint trajectory for blending trajectory.");
Expand Down
50 changes: 19 additions & 31 deletions moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,13 +63,6 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin
return false;
}

if (!robot_model->getJointModelGroup(group_name)->canSetStateFromIK(link_name))
{
RCLCPP_ERROR_STREAM(getLogger(),
"No valid IK solver exists for " << link_name << " in planning group " << group_name);
return false;
}

if (frame_id != robot_model->getModelFrame())
{
RCLCPP_ERROR_STREAM(getLogger(), "Given frame (" << frame_id << ") is unequal to model frame("
Expand All @@ -81,18 +74,22 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin
rstate.setVariablePositions(seed);

moveit::core::GroupStateValidityCallbackFn ik_constraint_function;
ik_constraint_function = [check_self_collision, scene](moveit::core::RobotState* robot_state,
const moveit::core::JointModelGroup* joint_group,
const double* joint_group_variable_values) {
return pilz_industrial_motion_planner::isStateColliding(check_self_collision, scene, robot_state, joint_group,
joint_group_variable_values);
};
if (check_self_collision)
{
ik_constraint_function = [scene](moveit::core::RobotState* robot_state,
const moveit::core::JointModelGroup* joint_group,
const double* joint_group_variable_values) {
return pilz_industrial_motion_planner::isStateColliding(scene, robot_state, joint_group,
joint_group_variable_values);
};
}

// call ik
if (rstate.setFromIK(robot_model->getJointModelGroup(group_name), pose, link_name, timeout, ik_constraint_function))
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group_name);
if (rstate.setFromIK(jmg, pose, link_name, timeout, ik_constraint_function))
{
// copy the solution
for (const auto& joint_name : robot_model->getJointModelGroup(group_name)->getActiveJointModelNames())
for (const auto& joint_name : jmg->getActiveJointModelNames())
{
solution[joint_name] = rstate.getVariablePosition(joint_name);
}
Expand Down Expand Up @@ -120,25 +117,22 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin
timeout);
}

bool pilz_industrial_motion_planner::computeLinkFK(const planning_scene::PlanningSceneConstPtr& scene,
const std::string& link_name,
bool pilz_industrial_motion_planner::computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name,
const std::map<std::string, double>& joint_state,
Eigen::Isometry3d& pose)
{ // take robot state from the current scene
moveit::core::RobotState rstate{ scene->getCurrentState() };

{
// check the reference frame of the target pose
if (!rstate.knowsFrameTransform(link_name))
if (!robot_state.knowsFrameTransform(link_name))
{
RCLCPP_ERROR_STREAM(getLogger(), "The target link " << link_name << " is not known by robot.");
return false;
}

rstate.setVariablePositions(joint_state);
robot_state.setVariablePositions(joint_state);

// update the frame
rstate.update();
pose = rstate.getFrameTransform(link_name);
robot_state.update();
pose = robot_state.getFrameTransform(link_name);

return true;
}
Expand Down Expand Up @@ -574,17 +568,11 @@ bool pilz_industrial_motion_planner::intersectionFound(const Eigen::Vector3d& p_
return ((p_current - p_center).norm() <= r) && ((p_next - p_center).norm() >= r);
}

bool pilz_industrial_motion_planner::isStateColliding(const bool test_for_self_collision,
const planning_scene::PlanningSceneConstPtr& scene,
bool pilz_industrial_motion_planner::isStateColliding(const planning_scene::PlanningSceneConstPtr& scene,
moveit::core::RobotState* rstate,
const moveit::core::JointModelGroup* const group,
const double* const ik_solution)
{
if (!test_for_self_collision)
{
return true;
}

rstate->setJointGroupPositions(group, ik_solution);
rstate->update();
collision_detection::CollisionRequest collision_req;
Expand Down
64 changes: 37 additions & 27 deletions moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,11 +121,6 @@ void TrajectoryGenerator::checkForValidGroupName(const std::string& group_name)
void TrajectoryGenerator::checkStartState(const moveit_msgs::msg::RobotState& start_state,
const std::string& group) const
{
if (start_state.joint_state.name.empty())
{
throw NoJointNamesInStartState("No joint names for state state given");
}

if (start_state.joint_state.name.size() != start_state.joint_state.position.size())
{
throw SizeMismatchInStartState("Joint state name and position do not match in start state");
Expand Down Expand Up @@ -158,19 +153,11 @@ void TrajectoryGenerator::checkStartState(const moveit_msgs::msg::RobotState& st
}

void TrajectoryGenerator::checkJointGoalConstraint(const moveit_msgs::msg::Constraints& constraint,
const std::vector<std::string>& expected_joint_names,
const std::string& group_name) const
{
for (const auto& joint_constraint : constraint.joint_constraints)
{
const std::string& curr_joint_name{ joint_constraint.joint_name };
if (std::find(expected_joint_names.cbegin(), expected_joint_names.cend(), curr_joint_name) ==
expected_joint_names.cend())
{
std::ostringstream os;
os << "Cannot find joint \"" << curr_joint_name << "\" from start state in goal constraint";
throw StartStateGoalStateMismatch(os.str());
}

if (!robot_model_->getJointModelGroup(group_name)->hasJointModel(curr_joint_name))
{
Expand All @@ -189,7 +176,8 @@ void TrajectoryGenerator::checkJointGoalConstraint(const moveit_msgs::msg::Const
}

void TrajectoryGenerator::checkCartesianGoalConstraint(const moveit_msgs::msg::Constraints& constraint,
const std::string& group_name) const
const moveit::core::RobotState& robot_state,
const moveit::core::JointModelGroup* const jmg) const
{
assert(constraint.position_constraints.size() == 1);
assert(constraint.orientation_constraints.size() == 1);
Expand All @@ -215,7 +203,8 @@ void TrajectoryGenerator::checkCartesianGoalConstraint(const moveit_msgs::msg::C
throw PositionOrientationConstraintNameMismatch(os.str());
}

if (!robot_model_->getJointModelGroup(group_name)->canSetStateFromIK(pos_constraint.link_name))
const auto& lm = robot_state.getRigidlyConnectedParentLinkModel(pos_constraint.link_name);
if (!lm || !jmg->canSetStateFromIK(lm->getName()))
{
std::ostringstream os;
os << "No IK solver available for link: \"" << pos_constraint.link_name << '\"';
Expand All @@ -229,8 +218,8 @@ void TrajectoryGenerator::checkCartesianGoalConstraint(const moveit_msgs::msg::C
}

void TrajectoryGenerator::checkGoalConstraints(
const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints,
const std::vector<std::string>& expected_joint_names, const std::string& group_name) const
const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints, const std::string& group_name,
const moveit::core::RobotState& rstate) const
{
if (goal_constraints.size() != 1)
{
Expand All @@ -247,21 +236,22 @@ void TrajectoryGenerator::checkGoalConstraints(

if (isJointGoalGiven(goal_con))
{
checkJointGoalConstraint(goal_con, expected_joint_names, group_name);
checkJointGoalConstraint(goal_con, group_name);
}
else
{
checkCartesianGoalConstraint(goal_con, group_name);
checkCartesianGoalConstraint(goal_con, rstate, robot_model_->getJointModelGroup(group_name));
}
}

void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRequest& req) const
void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRequest& req,
const moveit::core::RobotState& rstate) const
{
checkVelocityScaling(req.max_velocity_scaling_factor);
checkAccelerationScaling(req.max_acceleration_scaling_factor);
checkForValidGroupName(req.group_name);
checkStartState(req.start_state, req.group_name);
checkGoalConstraints(req.goal_constraints, req.start_state.joint_state.name, req.group_name);
checkGoalConstraints(req.goal_constraints, req.group_name, rstate);
}

void TrajectoryGenerator::setSuccessResponse(const moveit::core::RobotState& start_state, const std::string& group_name,
Expand Down Expand Up @@ -317,7 +307,7 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr&
res.planner_id = req.planner_id;
try
{
validateRequest(req);
validateRequest(req, scene->getCurrentState());
}
catch (const MoveItErrorCodeException& ex)
{
Expand All @@ -339,7 +329,7 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr&
return;
}

MotionPlanInfo plan_info;
MotionPlanInfo plan_info(scene, req);
try
{
extractMotionPlanInfo(scene, req, plan_info);
Expand All @@ -355,7 +345,7 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr&
trajectory_msgs::msg::JointTrajectory joint_trajectory;
try
{
plan(scene, req, plan_info, sampling_time, joint_trajectory);
plan(plan_info.start_scene, req, plan_info, sampling_time, joint_trajectory);
}
catch (const MoveItErrorCodeException& ex)
{
Expand All @@ -365,9 +355,29 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr&
return;
}

moveit::core::RobotState start_state(scene->getCurrentState());
moveit::core::robotStateMsgToRobotState(req.start_state, start_state, true);
setSuccessResponse(start_state, req.group_name, joint_trajectory, planning_begin, res);
setSuccessResponse(plan_info.start_scene->getCurrentState(), req.group_name, joint_trajectory, planning_begin, res);
}

TrajectoryGenerator::MotionPlanInfo::MotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
const planning_interface::MotionPlanRequest& req)
{
auto ps = scene->diff();
auto& start_state = ps->getCurrentStateNonConst();
// update start state from req
moveit::core::robotStateMsgToRobotState(scene->getTransforms(), req.start_state, start_state);
start_state.update();
start_scene = std::move(ps);

// initialize info.start_joint_position with active joint values from start_state
const double* positions = start_state.getVariablePositions();
for (const auto* jm : start_state.getRobotModel()->getJointModelGroup(req.group_name)->getActiveJointModels())
{
const auto& names = jm->getVariableNames();
for (std::size_t i = 0, j = jm->getFirstVariableIndex(); i < jm->getVariableCount(); ++i, ++j)
{
start_joint_position[names[i]] = positions[j];
}
}
}

} // namespace pilz_industrial_motion_planner
Loading

0 comments on commit 70e1aae

Please sign in to comment.