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; }