robowflex::TrajOptPlanner Class Reference

Robowflex Tesseract TrajOpt Planner. More...

#include <trajopt_planner.h>

Inheritance diagram for robowflex::TrajOptPlanner:

Classes
struct	Options
	Options structure with parameter values for TrajOpt planner. More...

Public Types
typedef std::pair< bool, bool >	PlannerResult
	Planner result: first->converged, second->collision_free. More...
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...

Public Member Functions
	TrajOptPlanner (const RobotPtr &robot, const std::string &group_name, const std::string &name="trajopt")
	Constructor. More...
bool	initialize (const std::string &manip)
	Initialize planner. The user must specify a chain group defined in the robot's srdf that contains all the links of the manipulator. More...
bool	initialize (const std::string &base_link, const std::string &tip_link)
	Initialize planner. All links between base_link and tip_link will be added to the manipulator. More...
Set and get TrajOpt parameters
void	setInitialTrajectory (const robot_trajectory::RobotTrajectoryPtr &init_trajectory)
	Set initial trajectory for TrajOpt and set init_type to GIVEN_TRAJ. More...
void	setInitialTrajectory (const trajopt::TrajArray &init_trajectory)
	Set initial trajectory for TrajOpt and set init_type to GIVEN_TRAJ. More...
void	setInitType (const trajopt::InitInfo::Type &init_type)
	Set type of initialization to use for the trajectory optimization. Current options are: STATIONARY JOINT_INTERPOLATED The user can also provide their own trajectory using setInitialTrajectory(). In such case, there is no need to call setInitType(). More...
const robot_trajectory::RobotTrajectoryPtr &	getTrajectory () const
	Get the trajectory that resulted in the last call to plan(). More...
const trajopt::TrajArray &	getTesseractTrajectory () const
	Get the trajectory that resulted in the last call to plan() in Tesseract format. More...
const std::vector< std::string > &	getEnvironmentLinks () const
	Get the link names in the tesseract KDL environment. More...
const std::vector< std::string > &	getManipulatorLinks () const
	Get the link names of the env manipulator. More...
const std::vector< std::string > &	getManipulatorJoints () const
	Get the joint names of the env manipulator. More...
double	getPlanningTime () const
	Get the time spent by the planner the last time it was called. More...
void	fixJoints (const std::vector< std::string > &joints)
	Constrain certain joints during optimization to their initial value. More...
Planning functions
planning_interface::MotionPlanResponse	plan (const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request) override
	Plan a motion given a request and a scene. More...
PlannerResult	plan (const SceneConstPtr &scene, const robot_state::RobotStatePtr &start_state, const robot_state::RobotStatePtr &goal_state)
	Plan a motion using a scene from start_state to a goal_state. More...
PlannerResult	plan (const SceneConstPtr &scene, const robot_state::RobotStatePtr &start_state, const RobotPose &goal_pose, const std::string &link)
	Plan a motion given a start_state, a goal_pose for a link and a scene. More...
PlannerResult	plan (const SceneConstPtr &scene, const std::unordered_map< std::string, double > &start_state, const RobotPose &goal_pose, const std::string &link)
	Plan a motion given a start_state, a goal_pose for a link and a scene. More...
PlannerResult	plan (const SceneConstPtr &scene, const RobotPose &start_pose, const std::string &start_link, const RobotPose &goal_pose, const std::string &goal_link)
	Plan a motion given a start_pose for start_link and a goal_pose for goal_link. More...
virtual PlannerResult	plan (const SceneConstPtr &scene, const robot_state::RobotStatePtr &start_state)
	Plan a motion using custom terms specified by the user. More...
std::vector< std::string >	getPlannerConfigs () const override
	Get 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...
Debugging Options
void	setWriteFile (bool file_write_cb, const std::string &file_path="")
	Set whether a report file (for the optimization) should be written or not. 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...

Public Attributes
struct robowflex::TrajOptPlanner::Options	options

Protected Member Functions
void	problemConstructionInfo (std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Create a TrajOpt problem construction info object with default values. More...
void	addVelocityCost (std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add velocity cost to the trajectory optimization. More...
void	addCollisionAvoidance (std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add collision avoidance cost to the trajectory optimization for all waypoints. More...
void	addStartState (const MotionRequestBuilderPtr &request, std::shared_ptr< trajopt::ProblemConstructionInfo > pci)
	Add a (joint) configuration constraint in the first waypoint taken from request. More...
void	addStartState (const robot_state::RobotStatePtr &start_state, std::shared_ptr< trajopt::ProblemConstructionInfo > pci)
	Add a (joint) configuration constraint start_state in the first waypoint. More...
void	addStartState (const std::unordered_map< std::string, double > &start_state, std::shared_ptr< trajopt::ProblemConstructionInfo > pci)
	Add a (joint) configuration constraint start_state in the first waypoint. More...
void	addStartPose (const RobotPose &start_pose, const std::string &link, std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add a cartesian pose constraint start_pose for link in the first waypoint. More...
void	addGoalState (const MotionRequestBuilderPtr &request, std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add a (joint) configuration constraint in the last waypoint taken from request. More...
void	addGoalState (const robot_state::RobotStatePtr &goal_state, std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add a (joint) configuration constraint goal_state in the last waypoint. More...
void	addGoalState (const std::vector< double > goal_state, std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add a (joint) configuration constraint goal_state in the last waypoint. More...
void	addGoalPose (const RobotPose &goal_pose, const std::string &link, std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add a cartesian pose constraint goal_pose for link in the last waypoint. More...
PlannerResult	solve (const SceneConstPtr &scene, const std::shared_ptr< trajopt::ProblemConstructionInfo > &pci)
	Solve SQP optimization problem. More...
sco::BasicTrustRegionSQPParameters	getTrustRegionSQPParameters () const
	Get parameters of the SQP. More...

Protected Attributes
robot_trajectory::RobotTrajectoryPtr	trajectory_
trajopt::TrajArray	tesseract_trajectory_
tesseract::tesseract_ros::KDLEnvPtr	env_
	KDL environment. More...
std::string	group_
	Name of group to plan for. More...
std::string	manip_
	Name of manipulator chain to check for collisions. More...
bool	cont_cc_ {true}
	Use continuous collision checking. More...
std::shared_ptr< std::ofstream >	stream_ptr_
	Debug file stream. More...
std::string	file_path_
	Path of debug file. More...
bool	file_write_cb_ {false}
	Whether to write a debug file or not. More...
trajopt::InitInfo::Type	init_type_ {trajopt::InitInfo::Type::STATIONARY}
trajopt::TrajArray	initial_trajectory_
	Initial trajectory (if any). More...
double	time_ {0.0}
	Time taken by the optimizer the last time it was called. More...
std::vector< int >	fixed_joints_
robot_state::RobotStatePtr	ref_state_
	Reference state to build moveit trajectory waypoints. 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

Robowflex Tesseract TrajOpt Planner.

Definition at line 33 of file trajopt_planner.h.

Member Typedef Documentation

PlannerResult

typedef std::pair<bool, bool> robowflex::TrajOptPlanner::PlannerResult

Planner result: first->converged, second->collision_free.

Definition at line 96 of file trajopt_planner.h.

Constructor & Destructor Documentation

TrajOptPlanner()

TrajOptPlanner::TrajOptPlanner	(	const RobotPtr &	robot,
		const std::string &	group_name,
		const std::string &	name = "trajopt"
	)

Constructor.

Parameters

[in]	robot	Robot to plan for.
[in]	group_name	Name of the (joint) group to plan for.
[in]	name	Name of planner.

Definition at line 24 of file trajopt_planner.cpp.

  : Planner(robot, name), group_(group_name)
{
}

Member Function Documentation

addCollisionAvoidance()

void TrajOptPlanner::addCollisionAvoidance ( std::shared_ptr< trajopt::ProblemConstructionInfo >  pci ) const

protected

Add collision avoidance cost to the trajectory optimization for all waypoints.

Parameters

[out]

pci

Pointer to problem construction info with collision avoidance added.

Definition at line 478 of file trajopt_planner.cpp.

{
    auto collision = std::make_shared<CollisionTermInfo>();
    collision->name = "collision_cost";
    collision->term_type = TT_COST;
    collision->continuous = options.use_cont_col_avoid;
    collision->first_step = 0;
    collision->last_step = options.num_waypoints - 1;
    collision->gap = options.collision_gap;
    collision->info = createSafetyMarginDataVector(pci->basic_info.n_steps, options.default_safety_margin,
                                                   options.default_safety_margin_coeffs);
    pci->cost_infos.push_back(collision);
}

addGoalPose()

void TrajOptPlanner::addGoalPose ( const RobotPose &  goal_pose, const std::string &  link, std::shared_ptr< trajopt::ProblemConstructionInfo >  pci  ) const

protected

Add a cartesian pose constraint goal_pose for link in the last waypoint.

Parameters

[in]	goal_pose	Desired goal cartesian pose of link.
[in]	link	Link that will be constrained to be in goal_pose in the last waypoint.
[out]	pci	Pointer to problem construction info with goal pose constraint added.

Definition at line 572 of file trajopt_planner.cpp.

{
    Eigen::Quaterniond rotation(goal_pose.linear());
    auto pose_constraint = std::make_shared<CartPoseTermInfo>();
    pose_constraint->term_type = TT_CNT;
    pose_constraint->link = link;
    pose_constraint->timestep = options.num_waypoints - 1;
    pose_constraint->xyz = goal_pose.translation();
    pose_constraint->wxyz = Eigen::Vector4d(rotation.w(), rotation.x(), rotation.y(), rotation.z());
    pose_constraint->pos_coeffs = Eigen::Vector3d::Constant(options.pose_cnt_pos_coeffs);
    pose_constraint->rot_coeffs = Eigen::Vector3d::Constant(options.pose_cnt_rot_coeffs);
    pose_constraint->name = "goal_pose_cnt_link_" + link;
    pci->cnt_infos.push_back(pose_constraint);
}

addGoalState() [1/3]

void robowflex::TrajOptPlanner::addGoalState ( const MotionRequestBuilderPtr &  request, std::shared_ptr< trajopt::ProblemConstructionInfo >  pci  ) const

protected

Add a (joint) configuration constraint in the last waypoint taken from request.

Parameters

[in]	request	Request motion planning problem.
[out]	pci	Pointer to problem construction info with goal state constraint added.

addGoalState() [2/3]

void robowflex::TrajOptPlanner::addGoalState ( const robot_state::RobotStatePtr &  goal_state, std::shared_ptr< trajopt::ProblemConstructionInfo >  pci  ) const

protected

Add a (joint) configuration constraint goal_state in the last waypoint.

Parameters

[in]	goal_state	Desired robot's goal state.
[out]	pci	Pointer to problem construction info with goal state constraint added.

addGoalState() [3/3]

void robowflex::TrajOptPlanner::addGoalState ( const std::vector< double >  goal_state, std::shared_ptr< trajopt::ProblemConstructionInfo >  pci  ) const

protected

Add a (joint) configuration constraint goal_state in the last waypoint.

Parameters

[in]	goal_state	Desired goal manipulator's joint values.
[out]	pci	Pointer to problem construction info with goal state constraint added.

addStartPose()

void TrajOptPlanner::addStartPose ( const RobotPose &  start_pose, const std::string &  link, std::shared_ptr< trajopt::ProblemConstructionInfo >  pci  ) const

protected

Add a cartesian pose constraint start_pose for link in the first waypoint.

Parameters

[in]	start_pose	Desired start cartesian pose of link.
[in]	link	Link that will be constrained to be in start_pose in the first waypoint.
[out]	pci	Pointer to problem construction info with start pose constraint added.

Definition at line 519 of file trajopt_planner.cpp.

{
    Eigen::Quaterniond rotation(start_pose.linear());
    auto pose_constraint = std::make_shared<CartPoseTermInfo>();
    pose_constraint->term_type = TT_CNT;
    pose_constraint->link = link;
    pose_constraint->timestep = 0;
    pose_constraint->xyz = start_pose.translation();
    pose_constraint->wxyz = Eigen::Vector4d(rotation.w(), rotation.x(), rotation.y(), rotation.z());
    pose_constraint->pos_coeffs = Eigen::Vector3d::Constant(options.pose_cnt_pos_coeffs);
    pose_constraint->rot_coeffs = Eigen::Vector3d::Constant(options.pose_cnt_rot_coeffs);
    pose_constraint->name = "start_pose_cnt_link_" + link;
    pci->cnt_infos.push_back(pose_constraint);
}

addStartState() [1/3]

void robowflex::TrajOptPlanner::addStartState ( const MotionRequestBuilderPtr &  request, std::shared_ptr< trajopt::ProblemConstructionInfo >  pci  )

protected

Add a (joint) configuration constraint in the first waypoint taken from request.

Parameters

[in]	request	Request motion planning problem.
[out]	pci	Pointer to problem construction info with start state constraint added.

addStartState() [2/3]

void robowflex::TrajOptPlanner::addStartState ( const robot_state::RobotStatePtr &  start_state, std::shared_ptr< trajopt::ProblemConstructionInfo >  pci  )

protected

Add a (joint) configuration constraint start_state in the first waypoint.

Parameters

[in]	start_state	Desired robot's start state.
[out]	pci	Pointer to problem construction info with start state constraint added.

addStartState() [3/3]

void robowflex::TrajOptPlanner::addStartState ( const std::unordered_map< std::string, double > &  start_state, std::shared_ptr< trajopt::ProblemConstructionInfo >  pci  )

protected

Add a (joint) configuration constraint start_state in the first waypoint.

Parameters

[in]	start_state	Desired start manipulator's joint names/values.
[out]	pci	Pointer to problem construction info with start state constraint added.

addVelocityCost()

void TrajOptPlanner::addVelocityCost ( std::shared_ptr< trajopt::ProblemConstructionInfo >  pci ) const

protected

Add velocity cost to the trajectory optimization.

Parameters

[out]

pci

Pointer to problem construction info with velocity cost added.

Definition at line 465 of file trajopt_planner.cpp.

{
    // Add joint velocity cost (without time) to penalize longer paths.
    auto jv = std::make_shared<JointVelTermInfo>();
    jv->targets = std::vector<double>(pci->kin->numJoints(), 0.0);
    jv->coeffs = std::vector<double>(pci->kin->numJoints(), options.joint_vel_coeffs);
    jv->term_type = TT_COST;
    jv->first_step = 0;
    jv->last_step = options.num_waypoints - 1;
    jv->name = "joint_velocity_cost";
    pci->cost_infos.push_back(jv);
}

fixJoints()

void TrajOptPlanner::fixJoints

(

const std::vector< std::string > &

joints

)

Constrain certain joints during optimization to their initial value.

Parameters

[in]

joints

Vector of joints to freeze.

Definition at line 173 of file trajopt_planner.cpp.

{
    if (!env_->hasManipulator(manip_))
        throw Exception(1, "There is no loaded manipulator!");
    else
    {
        const auto &joint_names = env_->getManipulator(manip_)->getJointNames();
        for (const auto &name : joints)
        {
            auto it = std::find(joint_names.begin(), joint_names.end(), name);
            if (it == joint_names.end())
                throw Exception(1, "One of the joints to be fixed does not exist");
            else
            {
                int index = std::distance(joint_names.begin(), it);
                fixed_joints_.push_back(index);
            }
        }
    }
}

getEnvironmentLinks()

const std::vector< std::string > & TrajOptPlanner::getEnvironmentLinks

(

)

const

Get the link names in the tesseract KDL environment.

Returns

Name of links in the KDL environment.

Definition at line 144 of file trajopt_planner.cpp.

{
    if (env_->checkInitialized())
        return env_->getLinkNames();
    else
        throw Exception(1, "KDL environment not initialized with robot links!");
}

getManipulatorJoints()

const std::vector< std::string > & TrajOptPlanner::getManipulatorJoints

(

)

const

Get the joint names of the env manipulator.

Returns

Name of joints in the manipulator.

Definition at line 160 of file trajopt_planner.cpp.

{
    if (env_->hasManipulator(manip_))
        return env_->getManipulator(manip_)->getJointNames();
    else
        throw Exception(1, "There is no loaded manipulator!");
}

getManipulatorLinks()

const std::vector< std::string > & TrajOptPlanner::getManipulatorLinks

(

)

const

Get the link names of the env manipulator.

Returns

Name of links in the manipulator.

Definition at line 152 of file trajopt_planner.cpp.

{
    if (env_->hasManipulator(manip_))
        return env_->getManipulator(manip_)->getLinkNames();
    else
        throw Exception(1, "There is no loaded manipulator!");
}

getPlannerConfigs()

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

overridevirtual

Get 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 427 of file trajopt_planner.cpp.

{
    std::vector<std::string> config;
    config.push_back("trajopt");
    return config;
}

getPlanningTime()

double TrajOptPlanner::getPlanningTime

(

)

const

Get the time spent by the planner the last time it was called.

Returns

Planning time.

Definition at line 168 of file trajopt_planner.cpp.

{
    return time_;
}

getTesseractTrajectory()

const trajopt::TrajArray & TrajOptPlanner::getTesseractTrajectory

(

)

const

Get the trajectory that resulted in the last call to plan() in Tesseract format.

Returns

Last trajectory computed using plan().

Definition at line 139 of file trajopt_planner.cpp.

{
    return tesseract_trajectory_;
}

getTrajectory()

const robot_trajectory::RobotTrajectoryPtr & TrajOptPlanner::getTrajectory

(

)

const

Get the trajectory that resulted in the last call to plan().

Returns

Last trajectory computed using plan().

Definition at line 134 of file trajopt_planner.cpp.

{
    return trajectory_;
}

getTrustRegionSQPParameters()

sco::BasicTrustRegionSQPParameters TrajOptPlanner::getTrustRegionSQPParameters ( ) const

protected

Get parameters of the SQP.

Returns

SQP parameters.

Definition at line 701 of file trajopt_planner.cpp.

{
    sco::BasicTrustRegionSQPParameters params;
 
    params.improve_ratio_threshold = options.improve_ratio_threshold;
    params.min_trust_box_size = options.min_trust_box_size;
    params.min_approx_improve = options.min_approx_improve;
    params.min_approx_improve_frac = options.min_approx_improve_frac;
    params.max_iter = options.max_iter;
    params.trust_shrink_ratio = options.trust_shrink_ratio;
    params.trust_expand_ratio = options.trust_expand_ratio;
    params.cnt_tolerance = options.cnt_tolerance;
    params.max_merit_coeff_increases = options.max_merit_coeff_increases;
    params.merit_coeff_increase_ratio = options.merit_coeff_increase_ratio;
    params.merit_error_coeff = options.merit_error_coeff;
    params.trust_box_size = options.trust_box_size;
 
    return params;
}

initialize() [1/2]

bool TrajOptPlanner::initialize	(	const std::string &	base_link,
		const std::string &	tip_link
	)

Initialize planner. All links between base_link and tip_link will be added to the manipulator.

Parameters

[in]	base_link	Base link of the manip.
[in]	tip_link	Tip link of the manip.

Returns

True if initialization succeded.

Definition at line 56 of file trajopt_planner.cpp.

{
    // Save manipulator name.
    manip_ = "manipulator";
 
    // Start KDL environment with the robot information.
    env_ = std::make_shared<tesseract::tesseract_ros::KDLEnv>();
 
    if (!robot_->getModelConst()->hasLinkModel(base_link))
    {
        RBX_ERROR("%s does not exist in robot description", base_link);
        return false;
    }
 
    if (!robot_->getModelConst()->hasLinkModel(tip_link))
    {
        RBX_ERROR("%s does not exist in robot description", tip_link);
        return false;
    }
 
    if (options.verbose)
        RBX_INFO("Adding manipulator %s from %s to %s", manip_, base_link, tip_link);
 
    TiXmlDocument srdf_doc;
    srdf_doc.Parse(robot_->getSRDFString().c_str());
 
    auto *group_element = new TiXmlElement("group");
    group_element->SetAttribute("name", manip_.c_str());
    srdf_doc.FirstChildElement("robot")->LinkEndChild(group_element);
 
    auto *chain_element = new TiXmlElement("chain");
    chain_element->SetAttribute("base_link", base_link.c_str());
    chain_element->SetAttribute("tip_link", tip_link.c_str());
    group_element->LinkEndChild(chain_element);
 
    srdf::ModelSharedPtr srdf;
    srdf.reset(new srdf::Model());
    srdf->initXml(*(robot_->getURDF()), &srdf_doc);
 
    if (!env_->init(robot_->getURDF(), srdf))
    {
        RBX_ERROR("Error loading robot %s", robot_->getName());
        return false;
    }
 
    // Check if manipulator was correctly loaded.
    if (!env_->hasManipulator(manip_))
    {
        RBX_ERROR("No manipulator found in KDL environment");
        return false;
    }
 
    // Initialize trajectory.
    trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(robot_->getModelConst(), group_);
 
    return true;
}

initialize() [2/2]

bool TrajOptPlanner::initialize

(

const std::string &

manip

)

Initialize planner. The user must specify a chain group defined in the robot's srdf that contains all the links of the manipulator.

Parameters

[in]

manip

Name of chain group with all the links of the manipulator.

Returns

True if initialization succeded.

Definition at line 29 of file trajopt_planner.cpp.

{
    // Save manipulator name.
    manip_ = manip;
 
    // Start KDL environment with the robot information.
    env_ = std::make_shared<tesseract::tesseract_ros::KDLEnv>();
 
    if (!env_->init(robot_->getURDF(), robot_->getSRDF()))
    {
        RBX_ERROR("Error loading robot %s", robot_->getName());
        return false;
    }
 
    // Check if manipulator was correctly loaded.
    if (!env_->hasManipulator(manip_))
    {
        RBX_ERROR("No manipulator found in KDL environment");
        return false;
    }
 
    // Initialize trajectory.
    trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(robot_->getModelConst(), group_);
 
    return true;
}

plan() [1/6]

planning_interface::MotionPlanResponse TrajOptPlanner::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 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 195 of file trajopt_planner.cpp.

{
    planning_interface::MotionPlanResponse res;
 
    // Extract start state.
    auto start_state = robot_->allocState();
    moveit::core::robotStateMsgToRobotState(request.start_state, *start_state);
    start_state->update(true);
 
    // Use the start state as reference state to build trajectory_.
    ref_state_ = std::make_shared<robot_state::RobotState>(*start_state);
 
    // Extract goal state.
    auto goal_state = robot_->allocState();
    if (request.goal_constraints.size() != 1)
    {
        RBX_ERROR("Ambiguous goal, %lu goal goal_constraints exist, returning default goal",
                  request.goal_constraints.size());
        res.error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
        return res;
    }
    if (request.goal_constraints[0].joint_constraints.empty())
    {
        RBX_ERROR("No joint constraints specified, returning default goal");
        res.error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
        return res;
    }
 
    std::map<std::string, double> variable_map;
    for (const auto &joint : request.goal_constraints[0].joint_constraints)
        variable_map[joint.joint_name] = joint.position;
 
    // Start state includes attached objects and values for the non-group links.
    moveit::core::robotStateMsgToRobotState(request.start_state, *goal_state);
    goal_state->setVariablePositions(variable_map);
    goal_state->update(true);
 
    // If planner runs until timeout, use the allowed_planning_time from the request.
    if (!options.return_first_sol)
        options.max_planning_time = request.allowed_planning_time;
 
    // Plan.
    auto result = plan(scene, start_state, goal_state);
    res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
    if (result.first and result.second)
    {
        res.planning_time_ = time_;
        res.trajectory_ = trajectory_;
        res.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
    }
 
    return res;
}

plan() [2/6]

TrajOptPlanner::PlannerResult TrajOptPlanner::plan ( const SceneConstPtr &  scene, const robot_state::RobotStatePtr &  start_state  )

virtual

Plan a motion using custom terms specified by the user.

Parameters

[in]	scene	A planning scene to compute the plan in.
[in]	start_state	Start state for the robot.

Returns

Planner result with convergence and collision status.

Reimplemented in CustomTrajOptPlanner.

Definition at line 421 of file trajopt_planner.cpp.

{
    throw Exception(1, "You need to implement virtual method TrajOptPlanner::plan() in your derived class");
}

plan() [3/6]

TrajOptPlanner::PlannerResult TrajOptPlanner::plan	(	const SceneConstPtr &	scene,
		const robot_state::RobotStatePtr &	start_state,
		const robot_state::RobotStatePtr &	goal_state
	)

Plan a motion using a scene from start_state to a goal_state.

Parameters

[in]	scene	Scene to plan for.
[in]	start_state	Start state for the robot.
[in]	goal_state	Goal state for the robot.

Returns

Planner result with convergence and collision status.

Definition at line 249 of file trajopt_planner.cpp.

{
    // Create the tesseract environment from the scene.
    if (hypercube::sceneToTesseractEnv(scene, env_))
    {
        // Attach bodies to KDL env.
        hypercube::addAttachedBodiesToTesseractEnv(ref_state_, env_);
 
        // Fill in the problem construction info and initialization.
        auto pci = std::make_shared<ProblemConstructionInfo>(env_);
        problemConstructionInfo(pci);
 
        // Add velocity cost.
        addVelocityCost(pci);
 
        // Add start state.
        addStartState(start_state, pci);
 
        // Add collision costs to all waypoints in the trajectory.
        options.default_safety_margin_coeffs = options.joint_state_safety_margin_coeffs;
        addCollisionAvoidance(pci);
 
        // Add goal state.
        addGoalState(goal_state, pci);
 
        return solve(scene, pci);
    }
 
    return PlannerResult(false, false);
}

plan() [4/6]

TrajOptPlanner::PlannerResult TrajOptPlanner::plan	(	const SceneConstPtr &	scene,
		const robot_state::RobotStatePtr &	start_state,
		const RobotPose &	goal_pose,
		const std::string &	link
	)

Plan a motion given a start_state, a goal_pose for a link and a scene.

Parameters

[in]	scene	A planning scene for the same robot to compute the plan in.
[in]	start_state	Start state for the robot.
[in]	goal_pose	Cartesian goal pose for link.
[in]	link	Link to find pose for.

Returns

Planner result with convergence and collision status.

Definition at line 282 of file trajopt_planner.cpp.

{
    if (init_type_ == InitInfo::Type::JOINT_INTERPOLATED)
    {
        RBX_ERROR("Straight line interpolation can not be done with a goal_pose.");
        return PlannerResult(false, false);
    }
 
    auto begin_it = env_->getManipulator(manip_)->getLinkNames().begin();
    auto end_it = env_->getManipulator(manip_)->getLinkNames().end();
    auto link_it = std::find(begin_it, end_it, link);
    if (link_it == end_it)
    {
        RBX_ERROR("Link %s is not part of robot manipulator", link);
        return PlannerResult(false, false);
    }
 
    // Use the start state as reference state to build trajectory_.
    ref_state_ = std::make_shared<robot_state::RobotState>(*start_state);
 
    // Create the tesseract environment from the scene.
    if (hypercube::sceneToTesseractEnv(scene, env_))
    {
        // Attach bodies to KDL env.
        hypercube::addAttachedBodiesToTesseractEnv(ref_state_, env_);
 
        // Fill in the problem construction info and initialization.
        auto pci = std::make_shared<ProblemConstructionInfo>(env_);
        problemConstructionInfo(pci);
 
        // Add velocity cost.
        addVelocityCost(pci);
 
        // Add start state
        addStartState(start_state, pci);
 
        // Add collision costs to all waypoints in the trajectory.
        addCollisionAvoidance(pci);
 
        // Add goal pose for link.
        addGoalPose(goal_pose, link, pci);
 
        return solve(scene, pci);
    }
 
    return PlannerResult(false, false);
}

plan() [5/6]

TrajOptPlanner::PlannerResult TrajOptPlanner::plan	(	const SceneConstPtr &	scene,
		const RobotPose &	start_pose,
		const std::string &	start_link,
		const RobotPose &	goal_pose,
		const std::string &	goal_link
	)

Plan a motion given a start_pose for start_link and a goal_pose for goal_link.

Parameters

[in]	scene	A planning scene to compute the plan in.
[in]	start_pose	Cartesian start pose for start_link.
[in]	start_link	Robot's link with start_pose.
[in]	goal_pose	Cartesian goal pose for goal_link.
[in]	goal_link	Robot's link with goal_pose.

Returns

Planner result with convergence and collision status.

Definition at line 376 of file trajopt_planner.cpp.

{
    if (init_type_ == InitInfo::Type::JOINT_INTERPOLATED)
    {
        RBX_ERROR("Straight line interpolation can not be done with a start_pose or a goal_pose");
        return PlannerResult(false, false);
    }
 
    auto begin_it = env_->getManipulator(manip_)->getLinkNames().begin();
    auto end_it = env_->getManipulator(manip_)->getLinkNames().end();
    auto start_it = std::find(begin_it, end_it, start_link);
    auto goal_it = std::find(begin_it, end_it, goal_link);
    if ((start_it == end_it) or (goal_it == end_it))
    {
        RBX_ERROR("Given links %s or %s are not part of robot manipulator", start_link, goal_link);
        return PlannerResult(false, false);
    }
 
    // Create the tesseract environment from the scene.
    if (hypercube::sceneToTesseractEnv(scene, env_))
    {
        // Fill in the problem construction info and initialization
        auto pci = std::make_shared<ProblemConstructionInfo>(env_);
        problemConstructionInfo(pci);
 
        // Add velocity cost.
        addVelocityCost(pci);
 
        // Add start_pose for start_link.
        addStartPose(start_pose, start_link, pci);
 
        // Add collision costs to all waypoints in the trajectory.
        addCollisionAvoidance(pci);
 
        // Add goal_pose for goal_link.
        addGoalPose(goal_pose, goal_link, pci);
 
        return solve(scene, pci);
    }
 
    return PlannerResult(false, false);
}

plan() [6/6]

TrajOptPlanner::PlannerResult TrajOptPlanner::plan	(	const SceneConstPtr &	scene,
		const std::unordered_map< std::string, double > &	start_state,
		const RobotPose &	goal_pose,
		const std::string &	link
	)

Plan a motion given a start_state, a goal_pose for a link and a scene.

Parameters

[in]	scene	A planning scene for the same robot to compute the plan in.
[in]	start_state	Start state for the robot.
[in]	goal_pose	Cartesian goal pose for link.
[in]	link	Link to find pose for.

Returns

Planner result with convergence and collision status.

Definition at line 332 of file trajopt_planner.cpp.

{
    if (init_type_ == InitInfo::Type::JOINT_INTERPOLATED)
    {
        RBX_ERROR("Straight line interpolation can not be done with a goal_pose.");
        return PlannerResult(false, false);
    }
 
    auto begin_it = env_->getManipulator(manip_)->getLinkNames().begin();
    auto end_it = env_->getManipulator(manip_)->getLinkNames().end();
    auto link_it = std::find(begin_it, end_it, link);
    if (link_it == end_it)
    {
        RBX_ERROR("Link %s is not part of robot manipulator", link);
        return PlannerResult(false, false);
    }
 
    // Create the tesseract environment from the scene.
    if (hypercube::sceneToTesseractEnv(scene, env_))
    {
        // Fill in the problem construction info and initialization.
        auto pci = std::make_shared<ProblemConstructionInfo>(env_);
        problemConstructionInfo(pci);
 
        // Add velocity cost.
        addVelocityCost(pci);
 
        // Add start state
        addStartState(start_state, pci);
 
        // Add collision costs to all waypoints in the trajectory.
        addCollisionAvoidance(pci);
 
        // Add goal pose for link.
        addGoalPose(goal_pose, link, pci);
 
        return solve(scene, pci);
    }
 
    return PlannerResult(false, false);
}

problemConstructionInfo()

void TrajOptPlanner::problemConstructionInfo ( std::shared_ptr< trajopt::ProblemConstructionInfo >  pci ) const

protected

Create a TrajOpt problem construction info object with default values.

Parameters

[out]

pci

Pointer to problem construction info initialized.

Definition at line 445 of file trajopt_planner.cpp.

{
    pci->basic_info.convex_solver = options.backend_optimizer;
    pci->kin = env_->getManipulator(manip_);
    pci->basic_info.n_steps = options.num_waypoints;
    pci->basic_info.manip = manip_;
    pci->basic_info.dt_lower_lim = options.dt_lower_lim;
    pci->basic_info.dt_upper_lim = options.dt_upper_lim;
    pci->basic_info.start_fixed = options.start_fixed;
    pci->basic_info.dofs_fixed = fixed_joints_;
    pci->basic_info.use_time = options.use_time;
    pci->init_info.type = init_type_;
    pci->init_info.dt = options.init_info_dt;
    if (init_type_ == InitInfo::Type::GIVEN_TRAJ)
        pci->init_info.data = initial_trajectory_;
 
    if (options.verbose)
        RBX_INFO("TrajOpt initialization: %d", init_type_);
}

setInitialTrajectory() [1/2]

void TrajOptPlanner::setInitialTrajectory

(

const robot_trajectory::RobotTrajectoryPtr &

init_trajectory

)

Set initial trajectory for TrajOpt and set init_type to GIVEN_TRAJ.

Parameters

[in]

init_trajectory

Trajectory to initialize TrajOpt.

Definition at line 114 of file trajopt_planner.cpp.

{
    hypercube::robotTrajToManipTesseractTraj(init_trajectory, manip_, env_, initial_trajectory_);
    init_type_ = InitInfo::Type::GIVEN_TRAJ;
}

setInitialTrajectory() [2/2]

void robowflex::TrajOptPlanner::setInitialTrajectory

(

const trajopt::TrajArray &

init_trajectory

)

Set initial trajectory for TrajOpt and set init_type to GIVEN_TRAJ.

Parameters

[in]

init_trajectory

Trajectory to initialize TrajOpt.

setInitType()

void TrajOptPlanner::setInitType

(

const trajopt::InitInfo::Type &

init_type

)

Set type of initialization to use for the trajectory optimization. Current options are: STATIONARY JOINT_INTERPOLATED The user can also provide their own trajectory using setInitialTrajectory(). In such case, there is no need to call setInitType().

Parameters

[in]

init_type

Type of initial trajectory to be used.

Definition at line 126 of file trajopt_planner.cpp.

{
    if (init_type != InitInfo::Type::GIVEN_TRAJ)
        init_type_ = init_type;
    else
        RBX_ERROR("init type can only be set to GIVEN_TRAJ calling the setInitialTrajectory() function");
}

setWriteFile()

void TrajOptPlanner::setWriteFile	(	bool	file_write_cb,
		const std::string &	file_path = ""
	)

Set whether a report file (for the optimization) should be written or not.

Parameters

[in]	file_write_cb	Whether to write the file or not.
[in]	file_path	File path

Definition at line 434 of file trajopt_planner.cpp.

{
    file_write_cb_ = file_write_cb;
    stream_ptr_ = std::make_shared<std::ofstream>();
    boost::filesystem::path path(file_path);
    path /= "file_output.csv";
    file_path_ = path.string();
    if (file_write_cb_)
        stream_ptr_->open(file_path_, std::ofstream::out | std::ofstream::trunc);
}

solve()

TrajOptPlanner::PlannerResult TrajOptPlanner::solve ( const SceneConstPtr &  scene, const std::shared_ptr< trajopt::ProblemConstructionInfo > &  pci  )

protected

Solve SQP optimization problem.

Parameters

[in]	scene	Scene to plan for.
[in]	pci	Pointer to problem construction info initialized.

Returns

Planner result with convergence and collision status.

Definition at line 588 of file trajopt_planner.cpp.

{
    PlannerResult planner_result(false, false);
 
    // Create optimizer and populate parameters.
    TrajOptProbPtr prob = ConstructProblem(*pci);
    sco::BasicTrustRegionSQP opt(prob);
    opt.setParameters(getTrustRegionSQPParameters());
 
    // Add write file callback.
    if (file_write_cb_)
    {
        if (!stream_ptr_->is_open())
            stream_ptr_->open(file_path_, std::ofstream::out | std::ofstream::trunc);
 
        opt.addCallback(WriteCallback(stream_ptr_, prob));
    }
 
    // If the planner needs to run more than once, add noise to the initial trajectory.
    if (!options.return_first_sol)
        options.perturb_init_traj = true;
 
    // Initialize.
    double total_time = 0.0;
    double best_cost = std::numeric_limits<double>::infinity();
 
    while (true)
    {
        // Perturb initial trajectory if needed.
        auto init_trajectory = prob->GetInitTraj();
        if (options.perturb_init_traj)
        {
            // Perturb all waypoints but start and goal.
            int rows = options.num_waypoints - 2;
            int cols = pci->kin->numJoints();
            double noise = options.noise_init_traj;
 
            init_trajectory.block(1, 0, rows, cols) += (Eigen::MatrixXd::Constant(rows, cols, -noise) +
                                                        2 * noise *
                                                            (Eigen::MatrixXd::Random(rows, cols) * 0.5 +
                                                             Eigen::MatrixXd::Constant(rows, cols, 0.5)));
        }
 
        opt.initialize(trajToDblVec(init_trajectory));
 
        // Optimize.
        ros::Time tStart = ros::Time::now();
        opt.optimize();
 
        // Measure and print time.
        double time = (ros::Time::now() - tStart).toSec();
        total_time += time;
        time_ = total_time;
 
        if (opt.results().status == sco::OptStatus::OPT_CONVERGED)
        {
            // Optimization problem converged.
            planner_result.first = true;
 
            // Check for collisions.
            auto tss_current_traj = getTraj(opt.x(), prob->GetVars());
            auto current_traj =
                std::make_shared<robot_trajectory::RobotTrajectory>(robot_->getModelConst(), group_);
            hypercube::manipTesseractTrajToRobotTraj(tss_current_traj, ref_state_, manip_, env_,
                                                     current_traj);
            auto const &ct = std::make_shared<Trajectory>(current_traj);
            bool is_ct_collision_free = ct->isCollisionFree(scene);
 
            // If trajectory is better than current, update the trajectory and cost.
            if ((opt.results().total_cost < best_cost) and is_ct_collision_free)
            {
                // Update best cost.
                best_cost = opt.results().total_cost;
 
                // Clear current trajectory.
                trajectory_->clear();
 
                // Update trajectory.
                tesseract_trajectory_ = tss_current_traj;
                trajectory_ = current_traj;
 
                // Solution is collision-free.
                planner_result.second = true;
            }
        }
 
        if (options.return_first_sol or
            (options.return_after_timeout and (total_time >= options.max_planning_time)) or
            (!options.return_after_timeout and
             (planner_result.second or (total_time >= options.max_planning_time))))
            break;
    }
 
    // Print status
    if (options.verbose)
    {
        RBX_INFO("OPTIMIZATION STATUS: %s", sco::statusToString(opt.results().status));
        RBX_INFO("TOTAL PLANNING TIME: %.3f", time_);
        RBX_INFO("COST: %.3f", best_cost);
        RBX_INFO("COLLISION STATUS: %s", (planner_result.second) ? "COLLISION FREE" : "IN COLLISION");
 
        if (planner_result.first)
            RBX_INFO("\n%s", tesseract_trajectory_);
    }
 
    // Write optimization results in file.
    if (file_write_cb_)
        stream_ptr_->close();
 
    return planner_result;
}

Member Data Documentation

cont_cc_

bool robowflex::TrajOptPlanner::cont_cc_ {true}

protected

Use continuous collision checking.

Definition at line 358 of file trajopt_planner.h.

env_

tesseract::tesseract_ros::KDLEnvPtr robowflex::TrajOptPlanner::env_

protected

KDL environment.

Definition at line 355 of file trajopt_planner.h.

file_path_

std::string robowflex::TrajOptPlanner::file_path_

protected

Path of debug file.

Definition at line 360 of file trajopt_planner.h.

file_write_cb_

bool robowflex::TrajOptPlanner::file_write_cb_ {false}

protected

Whether to write a debug file or not.

Definition at line 361 of file trajopt_planner.h.

fixed_joints_

std::vector<int> robowflex::TrajOptPlanner::fixed_joints_

protected

List of joints that need to be fixed, indexed in the order they appear in the manipulator.

Definition at line 366 of file trajopt_planner.h.

group_

std::string robowflex::TrajOptPlanner::group_

protected

Name of group to plan for.

Definition at line 356 of file trajopt_planner.h.

init_type_

trajopt::InitInfo::Type robowflex::TrajOptPlanner::init_type_ {trajopt::InitInfo::Type::STATIONARY}

protected

Type of initial trajectory.

Definition at line 362 of file trajopt_planner.h.

initial_trajectory_

trajopt::TrajArray robowflex::TrajOptPlanner::initial_trajectory_

protected

Initial trajectory (if any).

Definition at line 364 of file trajopt_planner.h.

manip_

std::string robowflex::TrajOptPlanner::manip_

protected

Name of manipulator chain to check for collisions.

Definition at line 357 of file trajopt_planner.h.

options

struct robowflex::TrajOptPlanner::Options robowflex::TrajOptPlanner::options

ref_state_

robot_state::RobotStatePtr robowflex::TrajOptPlanner::ref_state_

protected

Reference state to build moveit trajectory waypoints.

Definition at line 368 of file trajopt_planner.h.

stream_ptr_

std::shared_ptr<std::ofstream> robowflex::TrajOptPlanner::stream_ptr_

protected

Debug file stream.

Definition at line 359 of file trajopt_planner.h.

tesseract_trajectory_

trajopt::TrajArray robowflex::TrajOptPlanner::tesseract_trajectory_

protected

Last successful trajectory generated by the planner in Tesseract format.

Definition at line 353 of file trajopt_planner.h.

time_

double robowflex::TrajOptPlanner::time_ {0.0}

protected

Time taken by the optimizer the last time it was called.

Definition at line 365 of file trajopt_planner.h.

trajectory_

robot_trajectory::RobotTrajectoryPtr robowflex::TrajOptPlanner::trajectory_

protected

Last successful trajectory generated by the planner.

Definition at line 351 of file trajopt_planner.h.

---

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

• trajopt_planner.h
• trajopt_planner.cpp