void SimulatedCrops::refreshEncoders()
{
const SpecifiedTrajectory &traj = *current_trajectory_;
// Determines which segment of the trajectory to use
size_t seg = 0;
while (seg + 1 < traj.size() && traj[seg + 1].start_time <= ros::Time::now().toSec())
{
seg++;
}
for (size_t j = 0; j < traj[seg].splines.size(); j++)
{
double pos_t, vel_t, acc_t;
sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, ros::Time::now().toSec()
- traj[seg].start_time, pos_t, vel_t, acc_t);
motor_angles_[j] = pos_t;
motor_velocities_[j] = vel_t;
}
}
/**
* provides the "query_state" service
*/
bool JointTrajectoryActionController::queryStateService(pr2_controllers_msgs::QueryTrajectoryState::Request &req,
pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
{
ROS_WARN("queryStateService() called, this is not tested yet");
boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
traj_ptr = current_trajectory_;
if (!traj_ptr)
{
ROS_FATAL("The current trajectory can never be null");
return false;
}
const SpecifiedTrajectory &traj = *traj_ptr;
// Determines which segment of the trajectory to use
int seg = -1;
while (seg + 1 < (int)traj.size() && traj[seg + 1].start_time <= req.time.toSec())
{
++seg;
}
if (seg == -1)
return false;
resp.name.resize(joints_.size());
resp.position.resize(joints_.size());
resp.velocity.resize(joints_.size());
resp.acceleration.resize(joints_.size());
for (size_t j = 0; j < joints_.size(); ++j)
{
resp.name[j] = joints_[j];
sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, req.time.toSec() - traj[seg].start_time,
resp.position[j], resp.velocity[j], resp.acceleration[j]);
}
return true;
}
void JointTrajectoryActionController::update()
{
ros::Time time = ros::Time::now();
std::vector<double> q(joints_.size()), qd(joints_.size()), qdd(joints_.size());
boost::shared_ptr<const SpecifiedTrajectory> traj_ptr = current_trajectory_;
if (!traj_ptr)
ROS_FATAL("The current trajectory can never be null");
// Only because this is what the code originally looked like.
const SpecifiedTrajectory &traj = *traj_ptr;
if (traj.size() == 0)
{
ROS_ERROR("No segments in the trajectory");
return;
}
// ------ Finds the current segment
// Determines which segment of the trajectory to use.
int seg = -1;
while (seg + 1 < (int)traj.size() && traj[seg + 1].start_time <= time.toSec())
{
++seg;
}
if (seg == -1)
{
// ROS_ERROR("No earlier segments. First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
// return;
seg = 0;
}
// ------ Trajectory Sampling
for (size_t i = 0; i < q.size(); ++i)
{
sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration, time.toSec() - traj[seg].start_time,
q[i], qd[i], qdd[i]);
}
// ------ Calculate error
std::vector<double> error(joints_.size());
for (size_t i = 0; i < joints_.size(); ++i)
{
error[i] = katana_->getMotorAngles()[i] - q[i];
}
// ------ State publishing
pr2_controllers_msgs::JointTrajectoryControllerState msg;
// Message containing current state for all controlled joints
for (size_t j = 0; j < joints_.size(); ++j)
msg.joint_names.push_back(joints_[j]);
msg.desired.positions.resize(joints_.size());
msg.desired.velocities.resize(joints_.size());
msg.desired.accelerations.resize(joints_.size());
msg.actual.positions.resize(joints_.size());
msg.actual.velocities.resize(joints_.size());
// ignoring accelerations
msg.error.positions.resize(joints_.size());
msg.error.velocities.resize(joints_.size());
// ignoring accelerations
msg.header.stamp = time;
for (size_t j = 0; j < joints_.size(); ++j)
{
msg.desired.positions[j] = q[j];
msg.desired.velocities[j] = qd[j];
msg.desired.accelerations[j] = qdd[j];
msg.actual.positions[j] = katana_->getMotorAngles()[j];
msg.actual.velocities[j] = katana_->getMotorVelocities()[j];
// ignoring accelerations
msg.error.positions[j] = error[j];
msg.error.velocities[j] = katana_->getMotorVelocities()[j] - qd[j];
// ignoring accelerations
}
controller_state_publisher_.publish(msg);
// TODO: here we could publish feedback for the FollowJointTrajectory action; however,
// this seems to be optional (the PR2's joint_trajectory_action_controller doesn't do it either)
}
/**
* Checks if the given KNI trajectory fulfills all constraints.
*
* @param traj the KNI trajectory to sample
* @return
*/
bool JointTrajectoryActionController::validTrajectory(const SpecifiedTrajectory &traj)
{
const double MAX_SPEED = 2.21; // rad/s; TODO: should be same value as URDF
const double MIN_TIME = 0.01; // seconds; the KNI calculates time in 10ms units, so this is the minimum duration of a spline
const double EPSILON = 0.0001;
const double POSITION_TOLERANCE = 0.1; // rad
if (traj.size() > MOVE_BUFFER_SIZE)
ROS_WARN("new trajectory has %zu segments (move buffer size: %zu)", traj.size(), MOVE_BUFFER_SIZE);
// ------- check times
for (size_t seg = 0; seg < traj.size() - 1; seg++)
{
if (std::abs(traj[seg].start_time + traj[seg].duration - traj[seg + 1].start_time) > EPSILON)
{
ROS_ERROR("start time and duration of segment %zu do not match next segment", seg);
return false;
}
}
for (size_t seg = 0; seg < traj.size(); seg++)
{
if (traj[seg].duration < MIN_TIME)
{
ROS_WARN("duration of segment %zu is too small: %f", seg, traj[seg].duration);
// return false;
}
}
// ------- check start position
for (size_t j = 0; j < traj[0].splines.size(); j++)
{
if (std::abs(traj[0].splines[j].coef[0] - katana_->getMotorAngles()[j]) > POSITION_TOLERANCE)
{
ROS_ERROR("Initial joint angle of trajectory (%f) does not match current joint angle (%f) (joint %zu)", traj[0].splines[j].coef[0], katana_->getMotorAngles()[j], j);
return false;
}
}
// ------- check conditions at t = 0 and t = N
for (size_t j = 0; j < traj[0].splines.size(); j++)
{
if (std::abs(traj[0].splines[j].coef[1]) > EPSILON)
{
ROS_ERROR("Velocity at t = 0 is not 0: %f (joint %zu)", traj[0].splines[j].coef[1], j);
return false;
}
}
for (size_t j = 0; j < traj[traj.size() - 1].splines.size(); j++)
{
size_t seg = traj.size() - 1;
double vel_t, _;
sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, traj[seg].duration, _, vel_t, _);
if (std::abs(vel_t) > EPSILON)
{
ROS_ERROR("Velocity at t = N is not 0 (joint %zu)", j);
return false;
}
}
// ------- check for discontinuities between segments
for (size_t seg = 0; seg < traj.size() - 1; seg++)
{
for (size_t j = 0; j < traj[seg].splines.size(); j++)
{
double pos_t, vel_t, acc_t;
sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, traj[seg].duration, pos_t, vel_t, acc_t);
if (std::abs(traj[seg + 1].splines[j].coef[0] - pos_t) > EPSILON)
{
ROS_ERROR("Position discontinuity at end of segment %zu (joint %zu)", seg, j);
return false;
}
if (std::abs(traj[seg + 1].splines[j].coef[1] - vel_t) > EPSILON)
{
ROS_ERROR("Velocity discontinuity at end of segment %zu (joint %zu)", seg, j);
return false;
}
}
}
// ------- check position, speed, acceleration limits
for (double t = traj[0].start_time; t < traj.back().start_time + traj.back().duration; t += 0.01)
{
// Determines which segment of the trajectory to use
int seg = -1;
while (seg + 1 < (int)traj.size() && traj[seg + 1].start_time <= t)
{
++seg;
}
assert(seg >= 0);
for (size_t j = 0; j < traj[seg].splines.size(); j++)
{
double pos_t, vel_t, acc_t;
sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, t - traj[seg].start_time, pos_t, vel_t,
acc_t);
// TODO later: check position limits (min/max encoders)
if (std::abs(vel_t) > MAX_SPEED)
{
ROS_ERROR("Velocity %f too high at time %f (joint %zu)", vel_t, t, j);
return false;
}
// TODO later: check acceleration limits
}
}
return true;
}
boost::shared_ptr<SpecifiedTrajectory> JointTrajectoryActionController::calculateTrajectory(
const trajectory_msgs::JointTrajectory &msg)
{
boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr;
bool allPointsHaveVelocities = true;
// ------ Checks that the incoming segments have the right number of elements,
// determines which spline algorithm to use
for (size_t i = 0; i < msg.points.size(); i++)
{
if (msg.points[i].accelerations.size() != 0 && msg.points[i].accelerations.size() != joints_.size())
{
ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg.points[i].accelerations.size());
return new_traj_ptr; // return null pointer to signal error
}
if (msg.points[i].velocities.size() == 0)
{
// getCubicSplineCoefficients only works when the desired velocities are already given.
allPointsHaveVelocities = false;
}
else if (msg.points[i].velocities.size() != joints_.size())
{
ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg.points[i].velocities.size());
return new_traj_ptr; // return null pointer to signal error
}
if (msg.points[i].positions.size() != joints_.size())
{
ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg.points[i].positions.size());
return new_traj_ptr; // return null pointer to signal error
}
}
// ------ Correlates the joints we're commanding to the joints in the message
std::vector<int> lookup = makeJointsLookup(msg);
if (lookup.size() == 0)
return new_traj_ptr; // return null pointer to signal error
// ------ convert the boundary conditions to splines
new_traj_ptr.reset(new SpecifiedTrajectory);
SpecifiedTrajectory &new_traj = *new_traj_ptr;
size_t steps = msg.points.size() - 1;
ROS_DEBUG("steps: %zu", steps);
assert(steps > 0); // this is checked before
for (size_t i = 0; i < steps; i++)
{
Segment seg;
seg.splines.resize(joints_.size());
new_traj.push_back(seg);
}
for (size_t j = 0; j < joints_.size(); j++)
{
double times[steps + 1], positions[steps + 1], velocities[steps + 1], durations[steps], coeff0[steps],
coeff1[steps], coeff2[steps], coeff3[steps];
for (size_t i = 0; i < steps + 1; i++)
{
times[i] = msg.header.stamp.toSec() + msg.points[i].time_from_start.toSec();
positions[i] = msg.points[i].positions[lookup[j]];
if (allPointsHaveVelocities)
velocities[i] = msg.points[i].velocities[lookup[j]];
ROS_DEBUG("position %zu for joint %zu in message (= our joint %d): %f", i, j, lookup[j], positions[i]);
}
for (size_t i = 0; i < steps; i++)
{
durations[i] = times[i + 1] - times[i];
}
// calculate and store the coefficients
if (allPointsHaveVelocities)
{
ROS_DEBUG("Using getCubicSplineCoefficients()");
for (size_t i = 0; i < steps; ++i)
{
std::vector<double> coeff;
getCubicSplineCoefficients(positions[i], velocities[i], positions[i + 1], velocities[i + 1], durations[i],
coeff);
coeff0[i] = coeff[0];
coeff1[i] = coeff[1];
coeff2[i] = coeff[2];
coeff3[i] = coeff[3];
}
}
else
{
ROS_DEBUG("Using splineCoefficients()");
splineCoefficients(steps, times, positions, coeff0, coeff1, coeff2, coeff3);
}
for (size_t i = 0; i < steps; ++i)
{
new_traj[i].duration = durations[i];
new_traj[i].start_time = times[i];
new_traj[i].splines[j].target_position = positions[i + 1];
new_traj[i].splines[j].coef[0] = coeff0[i];
new_traj[i].splines[j].coef[1] = coeff1[i];
new_traj[i].splines[j].coef[2] = coeff2[i];
new_traj[i].splines[j].coef[3] = coeff3[i];
}
}
ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size());
for (size_t i = 0; i < std::min((size_t)20, new_traj.size()); i++)
{
ROS_DEBUG("Segment %2zu - start_time: %.3lf duration: %.3lf", i, new_traj[i].start_time, new_traj[i].duration);
for (size_t j = 0; j < new_traj[i].splines.size(); ++j)
{
ROS_DEBUG(" %.2lf %.2lf %.2lf %.2lf (%s)",
new_traj[i].splines[j].coef[0],
new_traj[i].splines[j].coef[1],
new_traj[i].splines[j].coef[2],
new_traj[i].splines[j].coef[3],
joints_[j].c_str());
}
}
// -------- sample trajectory and write to file
for (size_t j = 0; j < NUM_JOINTS; j++)
{
char filename[25];
sprintf(filename, "/tmp/trajectory-%zu.dat", j);
std::ofstream trajectory_file(filename, std::ios_base::out);
trajectory_file.precision(8);
for (double t = new_traj[0].start_time; t < new_traj.back().start_time + new_traj.back().duration; t += 0.01)
{
// Determines which segment of the trajectory to use
int seg = -1;
while (seg + 1 < (int)new_traj.size() && new_traj[seg + 1].start_time <= t)
{
++seg;
}
assert(seg >= 0);
double pos_t, vel_t, acc_t;
sampleSplineWithTimeBounds(new_traj[seg].splines[j].coef, new_traj[seg].duration, t - new_traj[seg].start_time,
pos_t, vel_t, acc_t);
trajectory_file << t - new_traj[0].start_time << " " << pos_t << " " << vel_t << " " << acc_t << std::endl;
}
trajectory_file.close();
}
return new_traj_ptr;
}
bool JointTrajectoryController::updateTrajectoryController(const SlaveMessageInput& actual,
SlaveMessageOutput& velocity)
{
time = boost::posix_time::microsec_clock::local_time();
boost::posix_time::time_duration dt = time - last_time;
last_time = time;
boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
current_trajectory_box_.Get(traj_ptr);
if (!traj_ptr || !this->isControllerActive)
{
this->isControllerActive = false;
// LOG(error) << "The current trajectory can never be null";
return false;
}
// Only because this is what the code originally looked like.
const SpecifiedTrajectory &traj = *traj_ptr;
// Determines which segment of the trajectory to use. (Not particularly realtime friendly).
int seg = -1;
while (seg + 1 < (int)traj.size() && traj[seg + 1].start_time < time)
{
++seg;
}
if (seg == -1)
{
if (traj.size() == 0)
LOG(error) << "No segments in the trajectory";
else
LOG(error) << "No earlier segments.";
return false;
}
if (seg == (int)traj.size() - 1 && (traj[seg].start_time + traj[seg].duration) < time)
{
LOG(trace) << "trajectory finished.";
this->isControllerActive = false;
velocity.value = 0;
velocity.controllerMode = VELOCITY_CONTROL;
return true;
}
// ------ Trajectory Sampling
duration = (double)traj[seg].duration.total_microseconds() / 1000.0 / 1000.0; //convert to seconds
time_till_seg_start = (double)(time - traj[seg].start_time).total_microseconds() / 1000.0 / 1000.0;
sampleSplineWithTimeBounds(traj[seg].spline.coef, duration, time_till_seg_start, targetPosition, targetVelocity,
targetAcceleration);
if (inverseDirection)
{
actualpose = -actual.actualPosition;
actualvel = -actual.actualVelocity;
}
else
{
actualpose = actual.actualPosition;
actualvel = actual.actualVelocity;
}
// ------ Trajectory Following
pose_error = ((actualpose / encoderTicksPerRound) * gearRatio * (2.0 * M_PI)) - targetPosition;
velocity_error = ((actualvel / 60.0) * gearRatio * 2.0 * M_PI) - targetVelocity;
velsetpoint = pid.updatePid(pose_error, velocity_error, dt);
velocity.value = (int32)round((velsetpoint / (gearRatio * 2.0 * M_PI)) * 60.0);
velocity.controllerMode = VELOCITY_CONTROL;
if (inverseDirection)
{
velocity.value = -velocity.value;
}
return true;
}
void JointTrajectoryController::setTrajectory(const JointTrajectory& input_traj)
{
if (input_traj.segments.size() == 0)
throw std::runtime_error("Invalid trajectory");
boost::posix_time::ptime time_now = boost::posix_time::microsec_clock::local_time();
boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
SpecifiedTrajectory &new_traj = *new_traj_ptr;
// ------ Grabs the trajectory that we're currently following.
boost::shared_ptr<const SpecifiedTrajectory> prev_traj_ptr;
current_trajectory_box_.Get(prev_traj_ptr);
if (!prev_traj_ptr)
{
throw std::runtime_error("The current trajectory can never be null");
return;
}
const SpecifiedTrajectory &prev_traj = *prev_traj_ptr;
// ------ Copies over the segments from the previous trajectory that are still useful.
// Useful segments are still relevant after the current time.
int first_useful = -1;
while (first_useful + 1 < (int)prev_traj.size() && prev_traj[first_useful + 1].start_time <= time_now)
{
++first_useful;
}
// Useful segments are not going to be completely overwritten by the message's splines.
int last_useful = -1;
while (last_useful + 1 < (int)prev_traj.size() && prev_traj[last_useful + 1].start_time < time_now)
{
++last_useful;
}
if (last_useful < first_useful)
first_useful = last_useful;
// Copies over the old segments that were determined to be useful.
for (int i = std::max(first_useful, 0); i <= last_useful; ++i)
{
new_traj.push_back(prev_traj[i]);
}
// We always save the last segment so that we know where to stop if
// there are no new segments.
if (new_traj.size() == 0)
new_traj.push_back(prev_traj[prev_traj.size() - 1]);
// ------ Determines when and where the new segments start
// Finds the end conditions of the final segment
Segment &last = new_traj[new_traj.size() - 1];
double prev_positions;
double prev_velocities;
double prev_accelerations;
LOG(debug) << "Initial conditions for new set of splines:";
sampleSplineWithTimeBounds(last.spline.coef, (double)last.duration.total_microseconds() / 1000.0 / 1000.0,
(double)last.start_time.time_of_day().total_microseconds() / 1000.0 / 1000.0,
prev_positions, prev_velocities, prev_accelerations);
// ROS_DEBUG(" %.2lf, %.2lf, %.2lf (%s)", prev_positions[i], prev_velocities[i],
// prev_accelerations[i], joints_[i]->joint_->name.c_str());
// ------ Tacks on the new segments
double positions;
double velocities;
double accelerations;
std::vector<boost::posix_time::time_duration> durations;
durations.push_back(input_traj.segments[0].time_from_start);
for (size_t i = 1; i < input_traj.segments.size(); ++i)
durations.push_back(input_traj.segments[i].time_from_start - input_traj.segments[i - 1].time_from_start);
for (unsigned int i = 0; i < input_traj.segments.size(); ++i)
{
Segment seg;
seg.start_time = input_traj.start_time + input_traj.segments[i].time_from_start;
seg.duration = durations[i];
positions = input_traj.segments[i].positions.value();
velocities = input_traj.segments[i].velocities.value();
accelerations = input_traj.segments[i].accelerations.value();
// Converts the boundary conditions to splines.
// if (prev_accelerations.size() > 0 && accelerations.size() > 0)
// {
getQuinticSplineCoefficients(prev_positions, prev_velocities, prev_accelerations, positions, velocities,
accelerations, (double)durations[i].total_microseconds() / 1000.0 / 1000.0,
seg.spline.coef);
/*
}
else if (prev_velocities.size() > 0 && velocities.size() > 0)
{
getCubicSplineCoefficients(
prev_positions[j], prev_velocities[j],
positions[j], velocities[j],
durations[i],
seg.splines[j].coef);
seg.splines[j].coef.resize(6, 0.0);
}
else
{
seg.splines[j].coef[0] = prev_positions[j];
if (durations[i] == 0.0)
seg.splines[j].coef[1] = 0.0;
else
seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
seg.splines[j].coef[2] = 0.0;
seg.splines[j].coef[3] = 0.0;
seg.splines[j].coef[4] = 0.0;
seg.splines[j].coef[5] = 0.0;
}
*/
// Pushes the splines onto the end of the new trajectory.
new_traj.push_back(seg);
// Computes the starting conditions for the next segment
prev_positions = positions;
prev_velocities = velocities;
prev_accelerations = accelerations;
}
// ------ Commits the new trajectory
if (!new_traj_ptr)
{
throw std::runtime_error("The new trajectory was null!");
return;
}
current_trajectory_box_.Set(new_traj_ptr);
LOG(debug) << "The new trajectory has " << new_traj.size() << " segments";
this->isControllerActive = true;
}
