robowflex::Trajectory Class Reference

The Trajectory class is a wrapper around MoveIt!'s robot_trajectory::RobotTrajectory, with extra convenience functions such interpolation and collision checking. There are also utilities to load and save trajectories from YAML files (toYAMLFile() and fromYAMLFile()). More...

#include <trajectory.h>

Inheritance diagram for robowflex::Trajectory:

Public Member Functions
	Trajectory (const RobotConstPtr &robot, const std::string &group)
	Constructor for an empty trajectory. More...
	Trajectory (const robot_trajectory::RobotTrajectory &trajectory)
	Constructor from moveit trajectory. More...
	Trajectory (const robot_trajectory::RobotTrajectoryPtr trajectory)
	Constructor from moveit trajectory. More...
IO
void	useMessage (const robot_state::RobotState &reference_state, const moveit_msgs::RobotTrajectory &msg)
	Set the trajectory to be the same as a message. More...
void	useMessage (const robot_state::RobotState &reference_state, const trajectory_msgs::JointTrajectory &msg)
	Set the trajectory to be the same as a message. More...
bool	toYAMLFile (const std::string &filename) const
	Dump a trajectory to a file. More...
bool	fromYAMLFile (const robot_state::RobotState &reference_state, const std::string &filename)
	Load a trajectory from a YAML file. More...
Getters and Setters
const robot_trajectory::RobotTrajectoryPtr &	getTrajectoryConst () const
	Get a const reference to the trajectory. More...
robot_trajectory::RobotTrajectoryPtr &	getTrajectory ()
	Get a reference to the trajectory. More...
moveit_msgs::RobotTrajectory	getMessage () const
	Get the message that describes the trajectory. More...
std::size_t	getNumWaypoints () const
	Returns the number of waypoints of the trajectory. More...
Adding and Modifying States
void	addSuffixWaypoint (const robot_state::RobotState &state, double dt=1.)
	Add a waypoint at the end of the trajectory. More...
Metrics
double	getLength (const PathMetric &metric={}) const
	Get the length of a path. Optionally, a metric can be specified other than the default (the L2 norm). More...
bool	isCollisionFree (const SceneConstPtr &scene) const
	Checks if a path is collsion free. More...
std::tuple< double, double, double >	getClearance (const SceneConstPtr &scene) const
	Get the average, minimum, and maximum clearance of a path. More...
double	getSmoothness (const PathMetric &metric={}) const
	Get the smoothness of a path relative to some metric. See internal function documentation for details. More...
std::map< std::string, double >	getFinalPositions () const
	Returns the joint positions from the last state in a planned trajectory in response. More...

Protected Attributes
robot_trajectory::RobotTrajectoryPtr	trajectory_

Processing Functions
bool	computeTimeParameterization (double max_velocity=1., double max_acceleration=1.)
	Computes the time parameterization of a path according to a desired max velocity or acceleration. More...
void	interpolate (unsigned int count)
	Insert a number of states in a path so that the path is made up of exactly count states. States are inserted uniformly (more states on longer segments). Changes are performed only if a path has less than count states. More...
std::vector< std::vector< double > >	vectorize () const
	Converts a trajectory into a vector of position vectors. The values are in the same order as reported by getJointNames(), which is consistent within MoveIt. More...
std::vector< std::string >	getJointNames () const
	Get the names of the variables that make up this trajectory, in the same order as in MoveIt JointModelGroup. More...
Trajectory &	append (const Trajectory &source, double dt, size_t start_index=0, size_t end_index=std::numeric_limits< std::size_t >::max())
	Adds a specified part of a trajectory to the end of the current trajectory. The default, when start_index or end_index are ommitted, is to add the whole trajectory. More...
static bool	computeTimeParameterization (robot_trajectory::RobotTrajectory &trajectory, double max_velocity=1., double max_acceleration=1.)
	Computes the time parameterization of a path according to a desired max velocity or acceleration. More...

Detailed Description

The Trajectory class is a wrapper around MoveIt!'s robot_trajectory::RobotTrajectory, with extra convenience functions such interpolation and collision checking. There are also utilities to load and save trajectories from YAML files (toYAMLFile() and fromYAMLFile()).

Definition at line 42 of file trajectory.h.

Constructor & Destructor Documentation

Trajectory() [1/3]

Trajectory::Trajectory	(	const RobotConstPtr &	robot,
		const std::string &	group
	)

Constructor for an empty trajectory.

Parameters

[in]	robot	Robot to construct trajectory for.
[in]	group	Planning group of the trajectory.

Definition at line 18 of file trajectory.cpp.

  : trajectory_(new robot_trajectory::RobotTrajectory(robot->getModelConst(), group))
{
}

Trajectory() [2/3]

Trajectory::Trajectory

(

const robot_trajectory::RobotTrajectory &

trajectory

)

Constructor from moveit trajectory.

Parameters

[in]

trajectory

Trajectory to initialize with.

Definition at line 23 of file trajectory.cpp.

  : trajectory_(new robot_trajectory::RobotTrajectory(trajectory))
{
}

Trajectory() [3/3]

Trajectory::Trajectory

(

const robot_trajectory::RobotTrajectoryPtr

trajectory

)

Constructor from moveit trajectory.

Parameters

[in]

trajectory

Trajectory to initialize with.

Definition at line 28 of file trajectory.cpp.

  : trajectory_(new robot_trajectory::RobotTrajectory(*trajectory))
{
}

Member Function Documentation

addSuffixWaypoint()

void Trajectory::addSuffixWaypoint	(	const robot_state::RobotState &	state,
		double	dt = 1.
	)

Add a waypoint at the end of the trajectory.

Parameters

[in]	state	State to add at end of trajectory.
[in]	dt	Time to this waypoint from previous.

Definition at line 64 of file trajectory.cpp.

{
    trajectory_->addSuffixWayPoint(state, dt);
}

append()

Trajectory & Trajectory::append	(	const Trajectory &	source,
		double	dt,
		size_t	start_index = 0,
		size_t	end_index = std::numeric_limits<std::size_t>::max()
	)

Adds a specified part of a trajectory to the end of the current trajectory. The default, when start_index or end_index are ommitted, is to add the whole trajectory.

Parameters

[in]	source	The trajectory containing the part to append to the end of current trajectory.
[in]	dt	Time step between last point in current traj and first point of append traj.
[in]	start_index	Index of first traj point of the part to append from the source traj.
[in]	end_index	Index of last traj point of the part to append from the source traj.

Definition at line 172 of file trajectory.cpp.

{
    trajectory_->append(*source.getTrajectoryConst(), dt, start_index, end_index);
    return *this;
}

computeTimeParameterization() [1/2]

bool Trajectory::computeTimeParameterization	(	double	max_velocity = 1.,
		double	max_acceleration = 1.
	)

Computes the time parameterization of a path according to a desired max velocity or acceleration.

Parameters

[in]	max_velocity	Maximum path velocity.
[in]	max_acceleration	Maximum path acceleration.

Returns

True on success, false on failure.

Definition at line 91 of file trajectory.cpp.

{
    trajectory_processing::IterativeParabolicTimeParameterization parameterizer;
    return parameterizer.computeTimeStamps(*trajectory_, max_velocity, max_acceleration);
}

computeTimeParameterization() [2/2]

bool Trajectory::computeTimeParameterization ( robot_trajectory::RobotTrajectory &  trajectory, double  max_velocity = 1., double  max_acceleration = 1.  )

static

Computes the time parameterization of a path according to a desired max velocity or acceleration.

Parameters

[in]	trajectory	to compute time parameterization.
[in]	max_velocity	Maximum path velocity.
[in]	max_acceleration	Maximum path acceleration.

Returns

True on success, false on failure.

Definition at line 97 of file trajectory.cpp.

{
    trajectory_processing::IterativeParabolicTimeParameterization parameterizer;
    return parameterizer.computeTimeStamps(trajectory, max_velocity, max_acceleration);
}

fromYAMLFile()

bool Trajectory::fromYAMLFile	(	const robot_state::RobotState &	reference_state,
		const std::string &	filename
	)

Load a trajectory from a YAML file.

Parameters

[in]	reference_state	A full state that contains the values for all the joints.
[in]	filename	Trajectory filename.

Returns

True on success.

Definition at line 54 of file trajectory.cpp.

{
    moveit_msgs::RobotTrajectory msg;
    if (!IO::YAMLFileToMessage(msg, filename))
        return false;
 
    useMessage(reference_state, msg);
    return true;
}

getClearance()

std::tuple< double, double, double > Trajectory::getClearance

(

const SceneConstPtr &

scene

)

const

Get the average, minimum, and maximum clearance of a path.

Parameters

[in]

scene

Scene to compute clearance to.

Returns

In order, the average, minimum, and maximum clearance of a path to a scene.

Definition at line 214 of file trajectory.cpp.

{
    double minimum = std::numeric_limits<double>::max();
    double maximum = 0;
    double average = 0;
 
    for (std::size_t k = 0; k < trajectory_->getWayPointCount(); ++k)
    {
        const auto &s = trajectory_->getWayPointPtr(k);
        double clearance = scene->distanceToCollision(*s);
        if (clearance > 0.0)
        {
            average += clearance;
            maximum = (maximum > clearance) ? maximum : clearance;
            minimum = (minimum < clearance) ? minimum : clearance;
        }
    }
 
    average /= trajectory_->getWayPointCount();
 
    return std::make_tuple(average, minimum, maximum);
}

getFinalPositions()

std::map< std::string, double > Trajectory::getFinalPositions

(

)

const

Returns the joint positions from the last state in a planned trajectory in response.

Returns

A map of joint name to joint position of the last state in response.

Definition at line 284 of file trajectory.cpp.

{
    const auto &last = trajectory_->getLastWayPoint();
 
    std::map<std::string, double> map;
 
    const auto &names = last.getVariableNames();
    const auto &values = last.getVariablePositions();
 
    for (std::size_t i = 0; i < names.size(); ++i)
        map.emplace(names[i], values[i]);
 
    return map;
}

getJointNames()

std::vector< std::string > Trajectory::getJointNames

(

)

const

Get the names of the variables that make up this trajectory, in the same order as in MoveIt JointModelGroup.

Returns

A vector of joint names in order.

Definition at line 167 of file trajectory.cpp.

{
    return getMessage().joint_trajectory.joint_names;
}

getLength()

double Trajectory::getLength

(

const PathMetric &

metric = {}

)

const

Get the length of a path. Optionally, a metric can be specified other than the default (the L2 norm).

Parameters

[in]

metric

An optional metric to use to compute the length of the path.

Returns

Length of the path according to the metric.

Definition at line 178 of file trajectory.cpp.

{
    double length = 0.0;
 
    for (std::size_t k = 1; k < trajectory_->getWayPointCount(); ++k)
    {
        const auto &s1 = trajectory_->getWayPoint(k - 1);
        const auto &s2 = trajectory_->getWayPoint(k);
 
        if (metric)
            length += metric(s1, s2);
        else
            length += s1.distance(s2);
    }
 
    return length;
}

getMessage()

moveit_msgs::RobotTrajectory Trajectory::getMessage

(

)

const

Get the message that describes the trajectory.

Returns

The trajectory message.

Definition at line 79 of file trajectory.cpp.

{
    moveit_msgs::RobotTrajectory msg;
    trajectory_->getRobotTrajectoryMsg(msg);
    return msg;
}

getNumWaypoints()

std::size_t Trajectory::getNumWaypoints

(

)

const

Returns the number of waypoints of the trajectory.

Returns

The numbers of waypoints of the trajectory.

Definition at line 86 of file trajectory.cpp.

{
    return trajectory_->getWayPointCount();
}

getSmoothness()

double Trajectory::getSmoothness

(

const PathMetric &

metric = {}

)

const

Get the smoothness of a path relative to some metric. See internal function documentation for details.

Parameters

[in]

metric

An optional metric to use to compute the length of the path segments.

Returns

Smoothness of the path.

(a + b);

Definition at line 237 of file trajectory.cpp.

{
    double smoothness = 0.0;
 
    auto distance =
        (metric) ? metric : [](const robot_state::RobotState &a, const robot_state::RobotState &b) {
            return a.distance(b);
        };
 
    // compute smoothness
    if (trajectory_->getWayPointCount() > 2)
    {
        double a = distance(trajectory_->getWayPoint(0), trajectory_->getWayPoint(1));
        for (std::size_t k = 2; k < trajectory_->getWayPointCount(); ++k)
        {
            // view the trajectory_->as a sequence of segments, and look at the triangles it forms:
            //          s1
            //          /\          s4
            //      a  /  \ b       |
            //        /    \        |
            //       /......\_______|
            //     s0    c   s2     s3
            //
 
            // use Pythagoras generalized theorem to find the cos of the angle between segments a and b
            double b = distance(trajectory_->getWayPoint(k - 1), trajectory_->getWayPoint(k));
            double cdist = distance(trajectory_->getWayPoint(k - 2), trajectory_->getWayPoint(k));
            double acos_value = (a * a + b * b - cdist * cdist) / (2.0 * a * b);
            if (acos_value > -1.0 && acos_value < 1.0)
            {
                // the smoothness is actually the outside angle of the one we compute
                double angle = (constants::pi - acos(acos_value));
 
                // and we normalize by the length of the segments
                double u = 2.0 * angle;  /// (a + b);
                smoothness += u * u;
            }
 
            a = b;
        }
 
        smoothness /= trajectory_->getWayPointCount();
    }
 
    return smoothness;
}

getTrajectory()

robot_trajectory::RobotTrajectoryPtr & Trajectory::getTrajectory

(

)

Get a reference to the trajectory.

Returns

The trajectory.

Definition at line 74 of file trajectory.cpp.

{
    return trajectory_;
}

getTrajectoryConst()

const robot_trajectory::RobotTrajectoryPtr & Trajectory::getTrajectoryConst

(

)

const

Get a const reference to the trajectory.

Returns

The trajectory.

Definition at line 69 of file trajectory.cpp.

{
    return trajectory_;
}

interpolate()

void Trajectory::interpolate

(

unsigned int

count

)

Insert a number of states in a path so that the path is made up of exactly count states. States are inserted uniformly (more states on longer segments). Changes are performed only if a path has less than count states.

Parameters

[in]

count

number of states to insert.

Definition at line 104 of file trajectory.cpp.

{
#if ROBOWFLEX_AT_LEAST_KINETIC
    if (count < getNumWaypoints() || trajectory_->getWayPointCount() < 2)
        return;
 
    // the remaining length of the path we need to add states along
    double total_length = getLength();
 
    // the Number of segments that exist in this path.
    const int n1 = getNumWaypoints() - 1;
    int added = 0;
 
    for (int seg = 0; seg < n1; ++seg)
    {
        // Last waypoint that has not been interpolated.
        int i = seg + added;
        auto s0 = trajectory_->getWayPointPtr(i);      // First point of the (uninterploated) segment
        auto s2 = trajectory_->getWayPointPtr(i + 1);  // Last point of the (uninterploated) segment
 
        // compute an approximate number of states the following segment needs to contain; this includes
        // endpoints
        double segment_length = s0->distance(*s2);
        int ns = (int)floor(0.5 + (double)count * segment_length / total_length) + 1;
 
        // if more than endpoints are needed
        if (ns > 2)
        {
            ns -= 2;  // subtract endpoints
 
            // compute intermediate states
            for (int j = 1; j < ns; j++)
            {
                // The state to be inserted
                auto s1 = std::make_shared<robot_state::RobotState>(trajectory_->getRobotModel());
                double dt = double(j) / double(ns);
 
                s0->interpolate(*s2, dt, *s1);
                s1->update(true);
                trajectory_->insertWayPoint(i + j, *s1, dt);
                // count how many stated have been added
                added++;
            }
        }
    }
 
    RBX_INFO("Added %d extra states in the trajectory", added);
    return;
 
#endif
    throw Exception(1, "Not Implemented");
}

isCollisionFree()

bool Trajectory::isCollisionFree

(

const SceneConstPtr &

scene

)

const

Checks if a path is collsion free.

Parameters

[in]

scene

Scene to collision check the path with.

Returns

True if the path is collision free in the scene.

Definition at line 196 of file trajectory.cpp.

{
    bool correct = true;
 
    for (std::size_t k = 0; k < trajectory_->getWayPointCount(); ++k)
    {
        auto s = trajectory_->getWayPointPtr(k);
        if (!s->satisfiesBounds())
            return false;
 
        const auto result = scene->checkCollision(*s);
        if (result.collision)
            return false;
    }
 
    return correct;
}

toYAMLFile()

bool Trajectory::toYAMLFile

(

const std::string &

filename

)

const

Dump a trajectory to a file.

Parameters

[in]

filename

Trajectory filename.

Returns

True on success.

Definition at line 45 of file trajectory.cpp.

{
    moveit_msgs::RobotTrajectory msg;
    trajectory_->getRobotTrajectoryMsg(msg);
 
    YAML::Node node = robowflex::IO::toNode(msg);
    return robowflex::IO::YAMLToFile(node, filename);
}

useMessage() [1/2]

void Trajectory::useMessage	(	const robot_state::RobotState &	reference_state,
		const moveit_msgs::RobotTrajectory &	msg
	)

Set the trajectory to be the same as a message.

Parameters

[in]	reference_state	A full state that contains the values for all the joints
[in]	msg	Message used to set the trajectory

Definition at line 33 of file trajectory.cpp.

{
    trajectory_->setRobotTrajectoryMsg(reference_state, msg);
}

useMessage() [2/2]

void Trajectory::useMessage	(	const robot_state::RobotState &	reference_state,
		const trajectory_msgs::JointTrajectory &	msg
	)

Set the trajectory to be the same as a message.

Parameters

[in]	reference_state	A full state that contains the values for all the joints.
[in]	msg	Message used to set the trajectory

Definition at line 39 of file trajectory.cpp.

{
    trajectory_->setRobotTrajectoryMsg(reference_state, msg);
}

vectorize()

std::vector< std::vector< double > > Trajectory::vectorize

(

)

const

Converts a trajectory into a vector of position vectors. The values are in the same order as reported by getJointNames(), which is consistent within MoveIt.

Returns

The trajectory in vector form.

Definition at line 157 of file trajectory.cpp.

{
    std::vector<std::vector<double>> traj_vec;
    const auto &msg = getMessage();
    for (const auto &p : msg.joint_trajectory.points)
        traj_vec.emplace_back(p.positions);
 
    return traj_vec;
}

Member Data Documentation

trajectory_

robot_trajectory::RobotTrajectoryPtr robowflex::Trajectory::trajectory_

protected

Definition at line 218 of file trajectory.h.

---

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

• trajectory.h
• trajectory.cpp