SimulatedCrops::SimulatedCrops() :
AbstractCrops()
{
// Creates a "hold current position" trajectory.
boost::shared_ptr<SpecifiedTrajectory> hold_ptr(new SpecifiedTrajectory(1));
SpecifiedTrajectory &hold = *hold_ptr;
hold[0].start_time = ros::Time::now().toSec() - 0.001;
hold[0].duration = 0.0;
hold[0].splines.resize(NUM_MOTORS);
for (size_t j = 0; j < NUM_MOTORS; ++j)
{
hold[0].splines[j].coef[0] = 0.000;
}
current_trajectory_ = hold_ptr;
}
/**
* Sets the current trajectory to a simple "hold position" trajectory
*/
void JointTrajectoryActionController::reset_trajectory_and_stop()
{
katana_->freezeRobot();
ros::Time time = ros::Time::now();
// Creates a "hold current position" trajectory.
// It's important that this trajectory is always there, because it will be used as a starting point for any new trajectory.
boost::shared_ptr<SpecifiedTrajectory> hold_ptr(new SpecifiedTrajectory(1));
SpecifiedTrajectory &hold = *hold_ptr;
hold[0].start_time = time.toSec() - 0.001;
hold[0].duration = 0.0;
hold[0].splines.resize(joints_.size());
for (size_t j = 0; j < joints_.size(); ++j)
hold[0].splines[j].coef[0] = katana_->getMotorAngles()[j];
current_trajectory_ = hold_ptr;
}
void
init_ptr_ (struct generic_s **pptr, struct generic_s *ptr)
{
hold_ptr (ptr);
*pptr = ptr;
}
