/*! * \brief Executes a valid trajectory. * * Move the arm to a goal configuration in Joint Space * \param goal JointTrajectoryGoalConstPtr */ void executeTrajectory(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal) { ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size()); if(!executing_) { while(current_operation_mode_ != "velocity") { ROS_INFO("waiting for arm to go to velocity mode"); usleep(100000); } traj_ = goal->trajectory; if(traj_.points.size() == 1) { traj_generator_->moveThetas(traj_.points[0].positions, q_current); } else { //Insert the current point as first point of trajectory, then generate SPLINE trajectory trajectory_msgs::JointTrajectoryPoint p; p.positions.resize(7); p.velocities.resize(7); p.accelerations.resize(7); for(unsigned int i = 0; i<7; i++) { p.positions.at(i) = q_current.at(i); p.velocities.at(i) = 0.0; p.accelerations.at(i) = 0.0; } std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator it; it = traj_.points.begin(); traj_.points.insert(it,p); traj_generator_->moveTrajectory(traj_, q_current); } executing_ = true; startposition_ = q_current; //TODO: std::cout << "Trajectory time: " << traj_end_time_ << " Trajectory step: " << traj_step_ << "\n"; } else //suspend current movement and start new one { } while(executing_) { sleep(1); } as_.setSucceeded(); }
bool srvCallback_setAcc(cob_trajectory_controller::SetFloat::Request &req, cob_trajectory_controller::SetFloat::Response &res) { ROS_INFO("Setting acceleration to %f", req.value.data); traj_generator_->SetPTPacc(req.value.data); res.success.data = true; return true; }
/*! * \brief Run the controller. * * The Controller generates desired velocities for every joints from the current positions. * */ void run() { if(executing_) { watchdog_counter = 0; if (as_.isPreemptRequested() || !ros::ok() || current_operation_mode_ != "velocity") { /// set the action state to preempted executing_ = false; traj_generator_->isMoving = false; ROS_INFO("Preempted trajectory action"); return; } std::vector<double> des_vel; if(traj_generator_->step(q_current, des_vel)) { if(!traj_generator_->isMoving) //Trajectory is finished { executing_ = false; } brics_actuator::JointVelocities target_joint_vel; target_joint_vel.velocities.resize(7); for(unsigned int i=0; i<7; i++) { std::stringstream joint_name; joint_name << "arm_" << (i+1) << "_joint"; target_joint_vel.velocities[i].joint_uri = joint_name.str(); target_joint_vel.velocities[i].unit = "rad"; target_joint_vel.velocities[i].value = des_vel.at(i); } /// send everything joint_vel_pub_.publish(target_joint_vel); } else { ROS_INFO("An controller error occured!"); executing_ = false; } } else { /// WATCHDOG TODO: don't always send if(watchdog_counter < 10) { sensor_msgs::JointState target_joint_position; target_joint_position.position.resize(7); brics_actuator::JointVelocities target_joint_vel; target_joint_vel.velocities.resize(7); for (unsigned int i = 0; i < 7; i += 1) { std::stringstream joint_name; joint_name << "arm_" << (i+1) << "_joint"; target_joint_vel.velocities[i].joint_uri = joint_name.str(); target_joint_position.position[i] = 0; target_joint_vel.velocities[i].unit = "rad"; target_joint_vel.velocities[i].value = 0; } joint_vel_pub_.publish(target_joint_vel); joint_pos_pub_.publish(target_joint_position); } watchdog_counter++; } }
void run() { if(executing_) { failure_ = false; watchdog_counter = 0; //if (as_follow_.isPreemptRequested() || !ros::ok() || current_operation_mode_ != "velocity") if (!ros::ok() || current_operation_mode_ != "velocity") { // set the action state to preempted executing_ = false; traj_generator_->isMoving = false; //as_.setPreempted(); failure_ = true; return; } if (as_follow_.isPreemptRequested()) { //as_follow_.setAborted() failure_ = true; preemted_ = true; ROS_INFO("Preempted trajectory action"); return; } std::vector<double> des_vel; if(traj_generator_->step(q_current, des_vel)) { if(!traj_generator_->isMoving) //Finished trajectory { executing_ = false; preemted_ = false; } brics_actuator::JointVelocities target_joint_vel; target_joint_vel.velocities.resize(DOF); for(int i=0; i<DOF; i++) { target_joint_vel.velocities[i].joint_uri = JointNames_[i].c_str(); target_joint_vel.velocities[i].unit = "rad"; target_joint_vel.velocities[i].value = des_vel.at(i); } //send everything joint_vel_pub_.publish(target_joint_vel); } else { ROS_INFO("An controller error occured!"); failure_ = true; executing_ = false; } } else { //WATCHDOG TODO: don't always send if(watchdog_counter < 10) { brics_actuator::JointVelocities target_joint_vel; target_joint_vel.velocities.resize(DOF); for (int i = 0; i < DOF; i += 1) { target_joint_vel.velocities[i].joint_uri = JointNames_[i].c_str(); target_joint_vel.velocities[i].unit = "rad"; target_joint_vel.velocities[i].value = 0; } joint_vel_pub_.publish(target_joint_vel); ROS_INFO("Publishing 0-vel (%d)", DOF); } watchdog_counter++; } }
void spawnTrajector(trajectory_msgs::JointTrajectory trajectory) { if(!executing_ || preemted_) { //set component to velocity mode cob_srvs::SetOperationMode opmode; opmode.request.operation_mode.data = "velocity"; srvClient_SetOperationMode.call(opmode); ros::Time begin = ros::Time::now(); while(current_operation_mode_ != "velocity") { ROS_INFO("waiting for component to go to velocity mode"); usleep(100000); //add timeout and set action to rejected if((ros::Time::now() - begin).toSec() > velocity_timeout_) { rejected_ = true; return; } } std::vector<double> traj_start; if(preemted_ == true) //Calculate trajectory for runtime modification of trajectories { ROS_INFO("There is a old trajectory currently running"); traj_start = traj_generator_->last_q; trajectory_msgs::JointTrajectory temp_traj; temp_traj = trajectory; //Insert the saved point as first point of trajectory, then generate SPLINE trajectory trajectory_msgs::JointTrajectoryPoint p; p.positions.resize(DOF); p.velocities.resize(DOF); p.accelerations.resize(DOF); for(int i = 0; i<DOF; i++) { p.positions.at(i) = traj_start.at(i); p.velocities.at(i) = 0.0; p.accelerations.at(i) = 0.0; } std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator it; it = temp_traj.points.begin(); temp_traj.points.insert(it,p); //Now insert the current as first point of trajectory, then generate SPLINE trajectory for(int i = 0; i<DOF; i++) { p.positions.at(i) = traj_generator_->last_q1.at(i); p.velocities.at(i) = 0.0; p.accelerations.at(i) = 0.0; } it = temp_traj.points.begin(); temp_traj.points.insert(it,p); for(int i = 0; i<DOF; i++) { p.positions.at(i) = traj_generator_->last_q2.at(i); p.velocities.at(i) = 0.0; p.accelerations.at(i) = 0.0; } it = temp_traj.points.begin(); temp_traj.points.insert(it,p); for(int i = 0; i<DOF; i++) { p.positions.at(i) = traj_generator_->last_q3.at(i); p.velocities.at(i) = 0.0; p.accelerations.at(i) = 0.0; } it = temp_traj.points.begin(); temp_traj.points.insert(it,p); for(int i = 0; i<DOF; i++) { p.positions.at(i) = q_current.at(i); p.velocities.at(i) = 0.0; p.accelerations.at(i) = 0.0; } it = temp_traj.points.begin(); temp_traj.points.insert(it,p); traj_generator_->isMoving = false ; traj_generator_->moveTrajectory(temp_traj, traj_start); } else //Normal calculation of trajectories { traj_start = q_current; trajectory_msgs::JointTrajectory temp_traj; temp_traj = trajectory; if(temp_traj.points.size() == 1) { traj_generator_->isMoving = false ; traj_generator_->moveThetas(temp_traj.points[0].positions, traj_start); } else { //Insert the current point as first point of trajectory, then generate SPLINE trajectory trajectory_msgs::JointTrajectoryPoint p; p.positions.resize(DOF); p.velocities.resize(DOF); p.accelerations.resize(DOF); for(int i = 0; i<DOF; i++) { p.positions.at(i) = traj_start.at(i); p.velocities.at(i) = 0.0; p.accelerations.at(i) = 0.0; } std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator it; it = temp_traj.points.begin(); temp_traj.points.insert(it,p); traj_generator_->isMoving = false ; traj_generator_->moveTrajectory(temp_traj, traj_start); } } executing_ = true; startposition_ = q_current; preemted_ = false; } else //suspend current movement and start new one { } while(executing_) { if(!preemted_) { usleep(1000); } else { return; } } }