robowflex::SimpleCartesianPlanner Class Reference

A simple motion planner that uses interpolation of the end-effector in Cartesian space to find a path. Valid configurations are found using IK. This planner is not complete and typically only works for small movements of the end-effector. More...

#include <planning.h>

Inheritance diagram for robowflex::SimpleCartesianPlanner:

Public Member Functions
	SimpleCartesianPlanner (const RobotPtr &robot, const std::string &name="")
	Constructor. More...
	SimpleCartesianPlanner (SimpleCartesianPlanner const &)=delete
void	operator= (SimpleCartesianPlanner const &)=delete
planning_interface::MotionPlanResponse	plan (const robot_state::RobotState &start, const Robot::IKQuery &request)
	Plan a Cartesian motion (interpolation) given a request and a scene from a start configuration. The scene, attempts, and timeout are used from request. More...
planning_interface::MotionPlanResponse	plan (const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request) override
	Plan a motion given a request and a scene. More...
void	setMaxStep (double position, double rotation)
	Set the maximum step size allowed by the planner between output waypoints. More...
void	setJumpThreshold (double prismatic, double revolute)
	Set the maximum difference in joint configurations allowed by the planner between output waypoints. More...
std::vector< std::string >	getPlannerConfigs () const override
	Return all planner configurations offered by this planner. Any of the configurations returned can be set as the planner for a motion planning query sent to plan(). More...
Public Member Functions inherited from robowflex::Planner
	Planner (const RobotPtr &robot, const std::string &name="")
	Constructor. Takes in a robot description and an optional namespace name. If name is specified, planner parameters are namespaced under the namespace of robot. More...
	Planner (Planner const &)=delete
void	operator= (Planner const &)=delete
virtual std::map< std::string, ProgressProperty >	getProgressProperties (const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request) const
	Retrieve the planner progress property map for this planner given a specific request. More...
const RobotPtr	getRobot () const
	Return the robot for this planner. More...
const std::string &	getName () const
	Get the name of the planner. More...
virtual void	preRun (const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request)
	This function is called before benchmarking. More...

Private Attributes
double	max_step_pos_ {constants::cart_pos_step_size}
	Max EE step size for position in meters. More...
double	max_step_rot_ {constants::cart_rot_step_size}
	Max EE step size for rotation in radians. More...
double	jump_threshold_pri_ {constants::cart_pos_jump_tol}
double	jump_threshold_rev_ {constants::cart_rot_jump_tol}

Additional Inherited Members
Public Types inherited from robowflex::Planner
using	ProgressProperty = std::function< std::string()>
	A function that returns the value of a planner property over the course of a run. More...
Protected Attributes inherited from robowflex::Planner
RobotPtr	robot_
	The robot to plan for. More...
IO::Handler	handler_
	The parameter handler for the planner. More...
const std::string	name_
	Namespace for the planner. More...

Detailed Description

A simple motion planner that uses interpolation of the end-effector in Cartesian space to find a path. Valid configurations are found using IK. This planner is not complete and typically only works for small movements of the end-effector.

Definition at line 184 of file robowflex_library/include/robowflex_library/planning.h.

Constructor & Destructor Documentation

SimpleCartesianPlanner() [1/2]

SimpleCartesianPlanner::SimpleCartesianPlanner	(	const RobotPtr &	robot,
		const std::string &	name = ""
	)

Constructor.

SimpleCartesianPlanner

Definition at line 100 of file robowflex_library/src/planning.cpp.

  : Planner(robot, name)
{
}

SimpleCartesianPlanner() [2/2]

robowflex::SimpleCartesianPlanner::SimpleCartesianPlanner ( SimpleCartesianPlanner const &  )

delete

Member Function Documentation

getPlannerConfigs()

std::vector< std::string > SimpleCartesianPlanner::getPlannerConfigs ( ) const

overridevirtual

Return all planner configurations offered by this planner. Any of the configurations returned can be set as the planner for a motion planning query sent to plan().

Returns

A vector of strings of planner configuration names.

Implements robowflex::Planner.

Definition at line 244 of file robowflex_library/src/planning.cpp.

{
    return std::vector<std::string>{"cartesian"};
}

operator=()

void robowflex::SimpleCartesianPlanner::operator= ( SimpleCartesianPlanner const &  )

delete

plan() [1/2]

planning_interface::MotionPlanResponse SimpleCartesianPlanner::plan	(	const robot_state::RobotState &	start,
		const Robot::IKQuery &	request
	)

Plan a Cartesian motion (interpolation) given a request and a scene from a start configuration. The scene, attempts, and timeout are used from request.

Parameters

[in]	start	Starting state.
[in]	request	The desired end-effector pose.

Returns

The motion planning response generated by the planner.

Definition at line 146 of file robowflex_library/src/planning.cpp.

{
    planning_interface::MotionPlanResponse response;
    if (request.tips.size() > 1)
    {
        RBX_ERROR("SimpleCartesianPlanner only supports queries with a single goal!");
        response.error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
        return response;
    }
 
    std::string tip = request.tips[0];
    if (tip.empty())
    {
        const auto &tips = robot_->getSolverTipFrames(request.group);
        if (tips.size() == 1)
            tip = tips[0];
        else
        {
            RBX_ERROR("Request needs tip frame name!");
            response.error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
            return response;
        }
    }
 
    // Get JMG, link model, and GSVCF
    const auto &model = robot_->getModelConst();
    const auto &jmg = model->getJointModelGroup(request.group);
    const auto &lm = model->getLinkModel(tip);
 
    moveit::core::GroupStateValidityCallbackFn gsvcf;
    if (request.scene)
        gsvcf = request.scene->getGSVCF(false);
 
    std::vector<robot_state::RobotStatePtr> traj;
 
    moveit::core::MaxEEFStep step(max_step_pos_, max_step_rot_);
    moveit::core::JumpThreshold jump(jump_threshold_rev_, jump_threshold_pri_);
 
    double time = 0;
    bool success = false;
    ros::WallTime start_time = ros::WallTime::now();
 
    for (std::size_t i = 0;                                             //
         not success                                                    //
         and ((request.attempts) ? i < request.attempts : true)         //
         and ((request.timeout > 0.) ? time < request.timeout : true);  //
         ++i)
    {
        auto state = start;
 
        RobotPose pose;
        request.sampleRegion(pose, 0);
 
#if ROBOWFLEX_HAS_CARTESIAN_INTERPOLATOR
        double percentage =                                             //
            moveit::core::CartesianInterpolator::computeCartesianPath(  //
                &state, jmg, traj, lm, pose, true, step, jump, gsvcf);
#else
        double percentage =              //
            state.computeCartesianPath(  //
                jmg, traj, lm, pose, true, step, jump, gsvcf);
#endif
 
        // Check if successful, output is percent of path computed.
        success = std::fabs(percentage - 1.) < constants::eps;
        time = (ros::WallTime::now() - start_time).toSec();
    }
 
    Trajectory output(robot_, request.group);
    for (const auto &state : traj)
        output.addSuffixWaypoint(*state);
 
    output.computeTimeParameterization();
 
    response.trajectory_ = output.getTrajectory();
    response.planning_time_ = time;
 
    if (success)
        response.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
    else
        response.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
 
    return response;
}

plan() [2/2]

planning_interface::MotionPlanResponse SimpleCartesianPlanner::plan ( const SceneConstPtr &  scene, const planning_interface::MotionPlanRequest &  request  )

overridevirtual

Plan a motion given a request and a scene.

Parameters

[in]	scene	A planning scene for the same robot_ to compute the plan in.
[in]	request	The motion planning request to solve.

Returns

The motion planning response generated by the planner.

Implements robowflex::Planner.

Definition at line 106 of file robowflex_library/src/planning.cpp.

{
    planning_interface::MotionPlanResponse temp;
    if (request.goal_constraints.size() > 1)
    {
        RBX_ERROR("SimpleCartesianPlanner only supports queries with a single goal!");
        temp.error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
        return temp;
    }
 
    const auto &goal = request.goal_constraints[0];
    if (not goal.joint_constraints.empty() or not goal.visibility_constraints.empty())
    {
        RBX_ERROR("SimpleCartesianPlanner only supports pose goals!");
        temp.error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
        return temp;
    }
 
    if (goal.position_constraints.size() != 1 and goal.orientation_constraints.size() != 1)
    {
        RBX_ERROR("SimpleCartesianPlanner requires single position and orientation constraint!");
        temp.error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
        return temp;
    }
 
    // Get sampleable region through IK Query
    const auto &pc = goal.position_constraints[0];
    const auto &oc = goal.orientation_constraints[0];
    Robot::IKQuery query(request.group_name, pc, oc);
    query.attempts = request.num_planning_attempts;
    query.timeout = request.allowed_planning_time;
    query.scene = scene;
 
    // Get starting state
    auto state = robot_->allocState();
    moveit::core::robotStateMsgToRobotState(request.start_state, *state);
 
    return plan(*state, query);
}

setJumpThreshold()

void SimpleCartesianPlanner::setJumpThreshold	(	double	prismatic,
		double	revolute
	)

Set the maximum difference in joint configurations allowed by the planner between output waypoints.

Parameters

[in]	prismatic	The new threshold for movement of prismatic joints.
[in]	revolute	The new threshold for movement of revolute joints.

Definition at line 238 of file robowflex_library/src/planning.cpp.

{
    jump_threshold_pri_ = prismatic;
    jump_threshold_rev_ = revolute;
}

setMaxStep()

void SimpleCartesianPlanner::setMaxStep	(	double	position,
		double	rotation
	)

Set the maximum step size allowed by the planner between output waypoints.

Parameters

[in]	position	The new step size for the position of the end-effector.
[in]	rotation	The new step size for the rotation of the end-effector.

Definition at line 232 of file robowflex_library/src/planning.cpp.

{
    max_step_pos_ = position;
    max_step_rot_ = rotation;
}

Member Data Documentation

jump_threshold_pri_

double robowflex::SimpleCartesianPlanner::jump_threshold_pri_ {constants::cart_pos_jump_tol}

private

Max jump for prismatic joints in meters.

Definition at line 230 of file robowflex_library/include/robowflex_library/planning.h.

jump_threshold_rev_

double robowflex::SimpleCartesianPlanner::jump_threshold_rev_ {constants::cart_rot_jump_tol}

private

Max jump for revolute joints in radians.

Definition at line 232 of file robowflex_library/include/robowflex_library/planning.h.

max_step_pos_

double robowflex::SimpleCartesianPlanner::max_step_pos_ {constants::cart_pos_step_size}

private

Max EE step size for position in meters.

Definition at line 228 of file robowflex_library/include/robowflex_library/planning.h.

max_step_rot_

double robowflex::SimpleCartesianPlanner::max_step_rot_ {constants::cart_rot_step_size}

private

Max EE step size for rotation in radians.

Definition at line 229 of file robowflex_library/include/robowflex_library/planning.h.

---

The documentation for this class was generated from the following files:

• robowflex_library/include/robowflex_library/planning.h
• robowflex_library/src/planning.cpp