Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

minor RobotTrajectory patches #2751

Merged
merged 4 commits into from
Jul 9, 2021
Merged

minor RobotTrajectory patches #2751

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
6ac4415
RobotTrajectory: add convenience constructor
v4hn Jul 6, 2021
ef0cca7
RobotTrajectory: chain setter support
v4hn Jul 6, 2021
77d4b23
RobotTrajectory: expand tests
v4hn Jul 6, 2021
7ac6c6b
use make_shared
v4hn Jul 6, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
68 changes: 45 additions & 23 deletions moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include <moveit_msgs/RobotTrajectory.h>
#include <moveit_msgs/RobotState.h>
#include <deque>
#include <memory>

namespace robot_trajectory
{
Expand All @@ -51,8 +52,15 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory); // Defines RobotTrajectoryPtr, ConstPtr,
class RobotTrajectory
{
public:
/** @brief construct a trajectory */
explicit RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model);

/** @brief construct a trajectory for the named JointModelGroup */
RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group);

/** @brief construct a trajectory for the JointModelGroup
* If group is nullptr this is equivalent to the first constructor
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry, late to the party, could you please explain (in another PR) in the comments what it actually means for a RobotTrajectory to have a group set (or what happens when you use the default constructor)

👍 for starting with the comments!

*/
RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const moveit::core::JointModelGroup* group);

/** Assignment operator, performing a shallow copy, i.e. copying waypoints by pointer */
Expand All @@ -76,7 +84,11 @@ class RobotTrajectory

const std::string& getGroupName() const;

void setGroupName(const std::string& group_name);
RobotTrajectory& setGroupName(const std::string& group_name)
{
group_ = robot_model_->getJointModelGroup(group_name);
return *this;
}

std::size_t getWayPointCount() const
{
Expand Down Expand Up @@ -134,11 +146,12 @@ class RobotTrajectory
return 0.0;
}

void setWayPointDurationFromPrevious(std::size_t index, double value)
RobotTrajectory& setWayPointDurationFromPrevious(std::size_t index, double value)
{
if (duration_from_previous_.size() <= index)
duration_from_previous_.resize(index + 1, 0.0);
duration_from_previous_[index] = value;
return *this;
}

bool empty() const
Expand All @@ -151,45 +164,48 @@ class RobotTrajectory
* \param state - current robot state
* \param dt - duration from previous
*/
void addSuffixWayPoint(const moveit::core::RobotState& state, double dt)
RobotTrajectory& addSuffixWayPoint(const moveit::core::RobotState& state, double dt)
{
addSuffixWayPoint(moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt);
return addSuffixWayPoint(std::make_shared<moveit::core::RobotState>(state), dt);
}

/**
* \brief Add a point to the trajectory
* \param state - current robot state
* \param dt - duration from previous
*/
void addSuffixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
RobotTrajectory& addSuffixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
{
state->update();
waypoints_.push_back(state);
duration_from_previous_.push_back(dt);
return *this;
}

void addPrefixWayPoint(const moveit::core::RobotState& state, double dt)
RobotTrajectory& addPrefixWayPoint(const moveit::core::RobotState& state, double dt)
{
addPrefixWayPoint(moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt);
return addPrefixWayPoint(std::make_shared<moveit::core::RobotState>(state), dt);
}

void addPrefixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
RobotTrajectory& addPrefixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
{
state->update();
waypoints_.push_front(state);
duration_from_previous_.push_front(dt);
return *this;
}

void insertWayPoint(std::size_t index, const moveit::core::RobotState& state, double dt)
RobotTrajectory& insertWayPoint(std::size_t index, const moveit::core::RobotState& state, double dt)
{
insertWayPoint(index, moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt);
return insertWayPoint(index, std::make_shared<moveit::core::RobotState>(state), dt);
}

void insertWayPoint(std::size_t index, const moveit::core::RobotStatePtr& state, double dt)
RobotTrajectory& insertWayPoint(std::size_t index, const moveit::core::RobotStatePtr& state, double dt)
{
state->update();
waypoints_.insert(waypoints_.begin() + index, state);
duration_from_previous_.insert(duration_from_previous_.begin() + index, dt);
return *this;
}

/**
Expand All @@ -202,12 +218,17 @@ class RobotTrajectory
* \param end_index - index of last traj point of the part to append from the source traj, the default is to add until
* the end of the source traj
*/
void append(const RobotTrajectory& source, double dt, size_t start_index = 0,
size_t end_index = std::numeric_limits<std::size_t>::max());
RobotTrajectory& append(const RobotTrajectory& source, double dt, size_t start_index = 0,
size_t end_index = std::numeric_limits<std::size_t>::max());

void swap(robot_trajectory::RobotTrajectory& other);

void clear();
RobotTrajectory& clear()
{
waypoints_.clear();
duration_from_previous_.clear();
return *this;
}

double getDuration() const;

Expand All @@ -222,17 +243,17 @@ class RobotTrajectory
point in the trajectory
to be constructed internally is obtained by copying the reference state and overwriting the content from a
trajectory point in \e trajectory. */
void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const trajectory_msgs::JointTrajectory& trajectory);
RobotTrajectory& setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const trajectory_msgs::JointTrajectory& trajectory);

/** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required
to contain the values
for all joints. For this reason a full starting state must be specified as reference (\e reference_state). Each
point in the trajectory
to be constructed internally is obtained by copying the reference state and overwriting the content from a
trajectory point in \e trajectory. */
void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::RobotTrajectory& trajectory);
RobotTrajectory& setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::RobotTrajectory& trajectory);

/** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required
to contain the values
Expand All @@ -241,13 +262,14 @@ class RobotTrajectory
using \e state. Each point in the trajectory to be constructed internally is obtained by copying the reference
state and overwriting the content
from a trajectory point in \e trajectory. */
void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const moveit_msgs::RobotState& state,
const moveit_msgs::RobotTrajectory& trajectory);
RobotTrajectory& setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::RobotState& state,
const moveit_msgs::RobotTrajectory& trajectory);

void reverse();
RobotTrajectory& reverse();

void unwind();
void unwind(const moveit::core::RobotState& state);
RobotTrajectory& unwind();
RobotTrajectory& unwind(const moveit::core::RobotState& state);

/** @brief Finds the waypoint indicies before and after a duration from start.
* @param The duration from start.
Expand Down
57 changes: 31 additions & 26 deletions moveit_core/robot_trajectory/src/robot_trajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,11 @@

namespace robot_trajectory
{
RobotTrajectory::RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model)
: robot_model_(robot_model), group_(nullptr)
{
}

RobotTrajectory::RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group)
: robot_model_(robot_model), group_(group.empty() ? nullptr : robot_model->getJointModelGroup(group))
{
Expand All @@ -66,11 +71,6 @@ RobotTrajectory::RobotTrajectory(const RobotTrajectory& other, bool deepcopy)
}
}

void RobotTrajectory::setGroupName(const std::string& group_name)
{
group_ = robot_model_->getJointModelGroup(group_name);
}

const std::string& RobotTrajectory::getGroupName() const
{
if (group_)
Expand Down Expand Up @@ -100,11 +100,11 @@ void RobotTrajectory::swap(RobotTrajectory& other)
duration_from_previous_.swap(other.duration_from_previous_);
}

void RobotTrajectory::append(const RobotTrajectory& source, double dt, size_t start_index, size_t end_index)
RobotTrajectory& RobotTrajectory::append(const RobotTrajectory& source, double dt, size_t start_index, size_t end_index)
{
end_index = std::min(end_index, source.waypoints_.size());
if (start_index >= end_index)
return;
return *this;
waypoints_.insert(waypoints_.end(), std::next(source.waypoints_.begin(), start_index),
std::next(source.waypoints_.begin(), end_index));
std::size_t index = duration_from_previous_.size();
Expand All @@ -113,9 +113,11 @@ void RobotTrajectory::append(const RobotTrajectory& source, double dt, size_t st
std::next(source.duration_from_previous_.begin(), end_index));
if (duration_from_previous_.size() > index)
duration_from_previous_[index] += dt;

return *this;
}

void RobotTrajectory::reverse()
RobotTrajectory& RobotTrajectory::reverse()
{
std::reverse(waypoints_.begin(), waypoints_.end());
for (moveit::core::RobotStatePtr& waypoint : waypoints_)
Expand All @@ -129,12 +131,14 @@ void RobotTrajectory::reverse()
std::reverse(duration_from_previous_.begin(), duration_from_previous_.end());
duration_from_previous_.pop_back();
}

return *this;
}

void RobotTrajectory::unwind()
RobotTrajectory& RobotTrajectory::unwind()
{
if (waypoints_.empty())
return;
return *this;

const std::vector<const moveit::core::JointModel*>& cont_joints =
group_ ? group_->getContinuousJointModels() : robot_model_->getContinuousJointModels();
Expand Down Expand Up @@ -164,12 +168,14 @@ void RobotTrajectory::unwind()
}
for (moveit::core::RobotStatePtr& waypoint : waypoints_)
waypoint->update();

return *this;
}

void RobotTrajectory::unwind(const moveit::core::RobotState& state)
RobotTrajectory& RobotTrajectory::unwind(const moveit::core::RobotState& state)
{
if (waypoints_.empty())
return;
return *this;

const std::vector<const moveit::core::JointModel*>& cont_joints =
group_ ? group_->getContinuousJointModels() : robot_model_->getContinuousJointModels();
Expand Down Expand Up @@ -210,12 +216,8 @@ void RobotTrajectory::unwind(const moveit::core::RobotState& state)
}
for (moveit::core::RobotStatePtr& waypoint : waypoints_)
waypoint->update();
}

void RobotTrajectory::clear()
{
waypoints_.clear();
duration_from_previous_.clear();
return *this;
}

void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory,
Expand Down Expand Up @@ -352,11 +354,11 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajec
}
}

void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const trajectory_msgs::JointTrajectory& trajectory)
RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const trajectory_msgs::JointTrajectory& trajectory)
{
// make a copy just in case the next clear() removes the memory for the reference passed in
const moveit::core::RobotState& copy = reference_state;
const moveit::core::RobotState copy(reference_state);
clear();
std::size_t state_count = trajectory.points.size();
ros::Time last_time_stamp = trajectory.header.stamp;
Expand All @@ -376,10 +378,12 @@ void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& refe
addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).toSec());
last_time_stamp = this_time_stamp;
}

return *this;
}

void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::RobotTrajectory& trajectory)
RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::RobotTrajectory& trajectory)
{
// make a copy just in case the next clear() removes the memory for the reference passed in
const moveit::core::RobotState& copy = reference_state;
Expand Down Expand Up @@ -423,15 +427,16 @@ void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& refe
addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).toSec());
last_time_stamp = this_time_stamp;
}
return *this;
}

void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::RobotState& state,
const moveit_msgs::RobotTrajectory& trajectory)
RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::RobotState& state,
const moveit_msgs::RobotTrajectory& trajectory)
{
moveit::core::RobotState st(reference_state);
moveit::core::robotStateMsgToRobotState(state, st);
setRobotTrajectoryMsg(st, trajectory);
return setRobotTrajectoryMsg(st, trajectory);
}

void RobotTrajectory::findWayPointIndicesForDurationAfterStart(const double& duration, int& before, int& after,
Expand Down
40 changes: 39 additions & 1 deletion moveit_core/robot_trajectory/test/test_robot_trajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@ class RobotTrajectoryTestFixture : public testing::Test
robot_model_ = moveit::core::loadTestingRobotModel(robot_model_name_);
robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
robot_state_->setToDefaultValues();
robot_state_->setVariableVelocity(/*index*/ 0, /*value*/ 1.0);
robot_state_->setVariableAcceleration(/*index*/ 0, /*value*/ -0.1);
robot_state_->update();
}

Expand All @@ -75,7 +77,7 @@ class RobotTrajectoryTestFixture : public testing::Test
double duration_from_previous = 0.1;
std::size_t waypoint_count = 5;
for (std::size_t ix = 0; ix < waypoint_count; ++ix)
trajectory->addSuffixWayPoint(robot_state_, duration_from_previous);
trajectory->addSuffixWayPoint(*robot_state_, duration_from_previous);
v4hn marked this conversation as resolved.
Show resolved Hide resolved
// Quick check that getDuration is working correctly
EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count)
<< "Generated trajectory duration incorrect";
Expand Down Expand Up @@ -152,6 +154,42 @@ TEST_F(RobotTrajectoryTestFixture, ModifyFirstWaypointByValue)
modifyFirstWaypointAndCheckTrajectory(trajectory);
}

TEST_F(RobotTrajectoryTestFixture, DoubleReverse)
{
robot_trajectory::RobotTrajectoryPtr trajectory;
initTestTrajectory(trajectory);
moveit_msgs::RobotTrajectory initial_trajectory_msg;
trajectory->getRobotTrajectoryMsg(initial_trajectory_msg);

trajectory->reverse().reverse();

moveit_msgs::RobotTrajectory edited_trajectory_msg;
trajectory->getRobotTrajectoryMsg(edited_trajectory_msg);

EXPECT_EQ(initial_trajectory_msg, edited_trajectory_msg);
}

TEST_F(RobotTrajectoryTestFixture, ChainEdits)
{
robot_trajectory::RobotTrajectoryPtr initial_trajectory;
initTestTrajectory(initial_trajectory);
moveit_msgs::RobotTrajectory initial_trajectory_msg;
initial_trajectory->getRobotTrajectoryMsg(initial_trajectory_msg);

robot_trajectory::RobotTrajectory trajectory(robot_model_);
trajectory.setGroupName(arm_jmg_name_)
.clear()
.setRobotTrajectoryMsg(*robot_state_, initial_trajectory_msg)
.reverse()
.addSuffixWayPoint(*robot_state_, 0.1)
.addPrefixWayPoint(*robot_state_, 0.1)
.insertWayPoint(1, *robot_state_, 0.1)
.append(*initial_trajectory, 0.1);

EXPECT_EQ(trajectory.getGroupName(), arm_jmg_name_);
EXPECT_EQ(trajectory.getWayPointCount(), initial_trajectory->getWayPointCount() * 2 + 3);
}

TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryShallowCopy)
{
bool deepcopy = false;
Expand Down