Program Listing for File trajectory.h
↰ Return to documentation for file (/tmp/ws/src/robot_controllers/robot_controllers/include/robot_controllers/trajectory.h)
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2014-2015, Fetch Robotics Inc.
* Copyright (c) 2013, Unbounded Robotics Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Unbounded Robotics nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
// Author: Michael Ferguson
#ifndef ROBOT_CONTROLLERS__TRAJECTORY_H_
#define ROBOT_CONTROLLERS__TRAJECTORY_H_
#include <iomanip>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "angles/angles.h"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "robot_controllers_interface/utils.h"
namespace robot_controllers
{
struct TrajectoryPoint
{
std::vector<double> q;
std::vector<double> qd;
std::vector<double> qdd;
double time;
};
struct Trajectory
{
std::vector<TrajectoryPoint> points;
size_t size() const
{
return points.size();
}
};
inline bool trajectoryFromMsg(const trajectory_msgs::msg::JointTrajectory& message,
const std::vector<std::string> joints,
const rclcpp::Time now,
Trajectory* trajectory)
{
// Find mapping of joint names into message joint names
std::vector<int> mapping(joints.size(), -1);
for (size_t j = 0; j < joints.size(); ++j)
{
for (size_t i = 0; i < message.joint_names.size(); ++i)
{
if (joints[j] == message.joint_names[i])
{
mapping[j] = i;
break;
}
}
if (mapping[j] == -1)
{
return false;
}
}
// Make sure trajectory is empty
trajectory->points.clear();
double start_time = robot_controllers_interface::msg_to_sec(message.header.stamp);
if (start_time == 0.0)
start_time = robot_controllers_interface::to_sec(now);
// Fill in Trajectory
for (size_t p = 0; p < message.points.size(); ++p)
{
TrajectoryPoint point;
for (size_t j = 0; j < joints.size(); ++j)
{
point.q.push_back(message.points[p].positions[mapping[j]]);
if (message.points[p].velocities.size() == message.points[p].positions.size())
point.qd.push_back(message.points[p].velocities[mapping[j]]);
if (message.points[p].accelerations.size() == message.points[p].positions.size())
point.qdd.push_back(message.points[p].accelerations[mapping[j]]);
}
point.time = start_time +
robot_controllers_interface::msg_to_sec(message.points[p].time_from_start);
trajectory->points.push_back(point);
}
// Parsed this
return true;
}
inline bool spliceTrajectories(const Trajectory& t1,
const Trajectory& t2,
const double time,
Trajectory * t)
{
// Need at least one point in t1 for the following code to work
if (t1.size() == 0)
{
return false;
}
// Need at least one point in t2 for the following code to work
if (t2.size() == 0)
{
*t = t1;
return true;
}
// Check sizes
size_t num_joints = t1.points[0].q.size();
bool has_velocities = (t1.points[0].qd.size() == num_joints) &&
(t2.points[0].qd.size() == num_joints);
bool has_accelerations = (t1.points[0].qdd.size() == num_joints) &&
(t2.points[0].qdd.size() == num_joints);
// Just to be sure
t->points.clear();
// When does t2 start?
double start_t2 = t2.points[0].time;
// Find points in t1 after time, but before start_t2
for (size_t p = 0; p < t1.size(); ++p)
{
if (t1.points[p].time >= time && t1.points[p].time < start_t2)
{
if (t1.points[p].time > time && t->size() == 0)
{
/* This is first point in trajectory, and is after our
start time, see if we can add one point in front of it */
if (p > 0)
t->points.push_back(t1.points[p-1]);
}
t->points.push_back(t1.points[p]);
}
}
// Add points from t2
for (size_t p = 0; p < t2.size(); ++p)
{
if (t2.points[p].time >= time)
{
if (t2.points[p].time > time && t->size() == 0)
{
/* This is first point in trajectory, and is after our
start time, see if we can add one point in front of it */
if (p > 0)
t->points.push_back(t2.points[p-1]);
else if (t1.size() > 0)
t->points.push_back(t1.points[t1.size()-1]);
}
t->points.push_back(t2.points[p]);
}
}
if (!has_accelerations)
{
// Remove any accelerations in output trajectory
for (size_t i = 0; i < t->points.size(); i++)
{
t->points[i].qdd.clear();
}
}
if (!has_velocities)
{
// Remove any velocities in output trajectory
for (size_t i = 0; i < t->points.size(); i++)
{
t->points[i].qd.clear();
}
}
return true;
}
inline void rosPrintTrajectory(rclcpp::Node::SharedPtr node, Trajectory& t)
{
RCLCPP_INFO_STREAM(node->get_logger(), "Trajectory with " << t.size() << " points:");
for (size_t p = 0; p < t.size(); ++p)
{
RCLCPP_INFO_STREAM(node->get_logger(), " Point " << p <<
" at " << std::setprecision (15) << t.points[p].time);
for (size_t j = 0; j < t.points[p].q.size(); ++j)
{
if (t.points[p].qdd.size() == t.points[p].q.size())
{
RCLCPP_INFO_STREAM(node->get_logger(), " " << std::setprecision (5) <<
t.points[p].q[j] << ", " << std::setprecision (5) <<
t.points[p].qd[j] << ", " << std::setprecision (5) <<
t.points[p].qdd[j]);
}
else if(t.points[p].q.size() == t.points[p].q.size())
{
RCLCPP_INFO_STREAM(node->get_logger(), " " << std::setprecision (5) <<
t.points[p].q[j] << ", " << std::setprecision (5) <<
t.points[p].qd[j]);
}
else
{
RCLCPP_INFO_STREAM(node->get_logger(), " " << std::setprecision (5) <<
t.points[p].q[j]);
}
}
}
}
inline bool windupTrajectory(std::vector<bool> continuous,
Trajectory& trajectory)
{
for (size_t p = 0; p < trajectory.size(); p++)
{
if (continuous.size() != trajectory.points[p].q.size())
{
// Size does not match
return false;
}
for (size_t j = 0; j < continuous.size(); j++)
{
if (continuous[j])
{
if (p > 0)
{
// Unwind by taking shortest path from previous point
double shortest = angles::shortest_angular_distance(trajectory.points[p-1].q[j],
trajectory.points[p].q[j]);
trajectory.points[p].q[j] = trajectory.points[p-1].q[j] + shortest;
}
else
{
// Start between -PI and PI
trajectory.points[p].q[j] = angles::normalize_angle(trajectory.points[p].q[j]);
}
}
}
}
return true;
}
inline bool unwindTrajectoryPoint(std::vector<bool> continuous,
TrajectoryPoint& p)
{
if (continuous.size() != p.q.size())
{
return false;
}
for (size_t j = 0; j < continuous.size(); j++)
{
if (continuous[j])
{
p.q[j] = angles::normalize_angle(p.q[j]);
}
}
return true;
}
class TrajectorySampler
{
public:
TrajectorySampler() {}
virtual ~TrajectorySampler() {}
virtual TrajectoryPoint sample(double time) = 0;
virtual double end_time() = 0;
virtual Trajectory getTrajectory() = 0;
private:
// You no copy...
TrajectorySampler(const TrajectorySampler&);
TrajectorySampler& operator=(const TrajectorySampler&);
};
} // namespace robot_controllers
#endif // ROBOT_CONTROLLERS__TRAJECTORY_H_