void startTrajectory(control_msgs::FollowJointTrajectoryGoal goal_elbow, control_msgs::FollowJointTrajectoryGoal goal_torso)
{
// When to start the trajectory: 'duration' from now
goal_elbow.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
goal_torso.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
traj_client_elbow_flex_->sendGoal(goal_elbow);
traj_client_torso_->sendGoal(goal_torso);
}
//! Sends the command to start a given trajectory
void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal,bool right_arm)
{
//Start the trjaectory immediately
goal.trajectory.header.stamp = ros::Time::now();
if(right_arm)
traj_client_r_->sendGoal(goal);
else
traj_client_l_->sendGoal(goal);
}
//! Sends the command to start a given trajectory
void startTrajectory(control_msgs::FollowJointTrajectoryGoal goal)
{
// When to start the trajectory: 1s from now
goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
traj_client_->sendGoal(goal);
if (!traj_client_->waitForResult(ros::Duration(10.0)))
{
traj_client_->cancelGoal();
ROS_INFO("Action did not finish before the time out.\n");
}
}
bool VirtualTrajClient::Sync(TrajClient& t)
{
bool res;
res=t.GetJointLimits(qmin,qmax);
assert(res);
res=t.GetVelocityLimits(vmax);
assert(res);
res=t.GetAccelerationLimits(amax);
assert(res);
res=t.GetDecelerationLimits(dmax);
assert(res);
qmin0=qmin;
qmax0=qmax;
vmax0=vmax;
amax0=amax;
dmax0=dmax;
maxSegments = t.GetMaxSegments();
maxRate = t.Rate();
milestones.resize(maxSegments+1);
times.resize(maxSegments+1);
curSegment = 0;
numSegments = 0;
//get current status
res = t.GetEndConfig(milestones[0]);
assert(res);
times[0] = t.GetTrajEndTime();
return true;
}
//! Initialize the action client and wait for action server to come up
RobotArm()
{
// tell the action client that we want to spin a thread by default
traj_client_r_ = new TrajClient("r_arm_controller/joint_trajectory_action", true);
traj_client_l_ = new TrajClient("l_arm_controller/joint_trajectory_action", true);
// wait for action server to come up
while(!traj_client_l_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the joint_trajectory_action server");
}
// wait for action server to come up
while(!traj_client_r_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the joint_trajectory_action server");
}
}
IKTrajectoryExecutor()
{
//tell the joint trajectory action client that we want
//to spin a thread by default
action_client = new TrajClient("joint_trajectory_action", true);
//wait for the action server to come up
while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0)))
{
ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
}
//register a service to input desired Cartesian trajectories
service = node.advertiseService("execute_cartesian_ik_trajectory",
&IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);
//have to specify the order of the joints we're sending in our
//joint trajectory goal, even if they're already on the param server
goal.trajectory.joint_names.push_back("shoulder_pan_joint");
goal.trajectory.joint_names.push_back("shoulder_lift_joint");
goal.trajectory.joint_names.push_back("elbow_joint");
goal.trajectory.joint_names.push_back("wrist_1_joint");
goal.trajectory.joint_names.push_back("wrist_2_joint");
goal.trajectory.joint_names.push_back("wrist_3_joint");
}
IKTrajectoryExecutor(){
//create a client function for the IK service
ik_client = node.serviceClient<kinematics_msgs::GetPositionIK>
(ARM_IK_NAME, true);
//wait for the various services to be ready
ROS_INFO("Waiting for services to be ready");
ros::service::waitForService(ARM_IK_NAME);
ROS_INFO("Services ready");
//tell the joint trajectory action client that we want
//to spin a thread by default
action_client = new TrajClient("l_arm_controller/joint_trajectory_action", true);
//wait for the action server to come up
while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
}
//register a service to input desired Cartesian trajectories
service = node.advertiseService("left_execute_cartesian_ik_trajectory",
&IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);
//have to specify the order of the joints we're sending in our
//joint trajectory goal, even if they're already on the param server
goal.trajectory.joint_names.push_back("l_shoulder_pan_joint");
goal.trajectory.joint_names.push_back("l_shoulder_lift_joint");
goal.trajectory.joint_names.push_back("l_upper_arm_roll_joint");
goal.trajectory.joint_names.push_back("l_elbow_flex_joint");
goal.trajectory.joint_names.push_back("l_forearm_roll_joint");
goal.trajectory.joint_names.push_back("l_wrist_flex_joint");
goal.trajectory.joint_names.push_back("l_wrist_roll_joint");
}
bool execute_joint_trajectory(std::vector<double *> joint_trajectory, double req_time, double msg_time)
{
int i, j;
int trajectorylength = joint_trajectory.size();
goal.trajectory.points.resize(trajectorylength);
goal.trajectory.points[0].positions.resize(6);
goal.trajectory.points[0].velocities.resize(6);
double time_from_start = 0.0;
goal.trajectory.points[0].positions.resize(6);
goal.trajectory.points[0].velocities.resize(6);
// Velocities not used by UR5 servoj command
for(j=0; j<6; j++)
{
goal.trajectory.points[0].positions[j] = joint_trajectory[0][j];
goal.trajectory.points[0].velocities[j] = 0.0;
}
double max_joint_move = 0;
for(j=0; j<6; j++)
{
double joint_move = fabs(goal.trajectory.points[0].positions[j]
- current_angles[j]);
if(joint_move > max_joint_move) max_joint_move = joint_move;
}
double limit_time = max_joint_move/MAX_JOINT_VEL;
// ROS_INFO("max_joint_move: %0.3f, seconds: %0.3f", max_joint_move, seconds);
if (req_time < limit_time){
time_from_start += limit_time;
}
else {
time_from_start += req_time;
}
goal.trajectory.points[0].time_from_start =
ros::Duration(time_from_start);
//when to start the trajectory
// goal.trajectory.header.stamp = ros::Time::now();
// ROS_INFO("Sending goal to joint_trajectory_action");
double time2 = ros::Time::now().toSec();
ROS_INFO_STREAM("Time elapsed: " << (time2 - msg_time ));
action_client->sendGoal(goal);
// Servoing: need interrupt commands
// action_client->waitForResult();
return 1;
}
// Initialize the action client and wait for the action server to come up
RobotArm()
{
// tell the action client that we want to spin a thread by default
traj_client_elbow_flex_ = new TrajClient("r_elbow_flex_controller/follow_joint_trajectory", true);
traj_client_torso_ = new TrajClient("/torso_lift_controller/follow_joint_trajectory", true);
// wait for action server to come up
while (!traj_client_elbow_flex_->waitForServer(ros::Duration(5.0)))
{
ROS_INFO("Waiting for the r_elbow_flex_controller/follow_joint_trajectory server");
}
while (!traj_client_torso_->waitForServer(ros::Duration(5.0)))
{
ROS_INFO("Waiting for the r_elbow_flex_controller/follow_joint_trajectory server");
}
ROS_INFO("Works for now");
}
//! Initialize the action client and wait for action server to come up
MoveInJoints()
{
// tell the action client that we want to spin a thread by default
traj_client_ = new TrajClient("/move_iri_wam", true);
// wait for action server to come up
while(!traj_client_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_iri_wam server");
}
}
//! Initialize the action client and wait for action server to come up
RobotArm():srv_name_("/iri_wam/iri_wam_controller/joints_move") {
client = n.serviceClient<iri_common_drivers_msgs::QueryJointsMovement>(this->srv_name_.c_str());
// tell the action client that we want to spin a thread by default
traj_client_ = new TrajClient("/iri_wam/iri_wam_controller/follow_joint_trajectory", true);
// wait for action server to come up
while(!traj_client_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the joint_trajectory_action server");
}
}
//! Initialize the action client and wait for action server to come up
RobotArm()
{
// tell the action client that we want to spin a thread by default
traj_client_ = new TrajClient("/wam_wrapper/DMPTracker", true);
//initialize the topic to send new goals
pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/wam_wrapper/DMPTrackerNewGoal", 1);
// wait for action server to come up
while(!traj_client_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the joint_trajectory_action server");
}
}
//! Sends the command to start a given trajectory
void startTrajectory()
{
iri_wam_common_msgs::DMPTrackerGoal DMPgoal;
DMPgoal.initial.positions.resize(7);
DMPgoal.initial.positions[0] = -0.11976;
DMPgoal.initial.positions[1] = -1.84794;
DMPgoal.initial.positions[2] = 0.285349;
DMPgoal.initial.positions[3] = 2.84315;
DMPgoal.initial.positions[4] = -0.310117;
DMPgoal.initial.positions[5] = -1.21896;
DMPgoal.initial.positions[6] = 0.0192133;
DMPgoal.goal.positions.resize(7);
DMPgoal.goal.positions[0] = 0.160557;
DMPgoal.goal.positions[1] = -1.91039;
DMPgoal.goal.positions[2] = -0.664568;
DMPgoal.goal.positions[3] = 2.9184;
DMPgoal.goal.positions[4] = -0.5448;
DMPgoal.goal.positions[5] = -0.812694;
DMPgoal.goal.positions[6] = -0.471291;
// When to start the trajectory: 1s from now
//goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
traj_client_->sendGoal(DMPgoal);
for (int i=0;i<10;i++) {
sleep(0.5);
trajectory_msgs::JointTrajectoryPoint DMPnew_goal;
DMPnew_goal.positions.resize(7);
DMPnew_goal.positions[0] = -0.11976;
DMPnew_goal.positions[1] = -1.84794;
DMPnew_goal.positions[2] = 0.285349-i*0.02;
DMPnew_goal.positions[3] = 2.84315;
DMPnew_goal.positions[4] = -0.310117;
DMPnew_goal.positions[5] = -1.21896;
DMPnew_goal.positions[6] = 0.0192133;
pub.publish(DMPnew_goal);
}
}
void init (ros::NodeHandle* nh) {
ik_client = nh->serviceClient<moveit_msgs::GetPositionIK>
(ARM_IK_NAME, true);
ROS_INFO("Waiting for services to be ready");
ros::service::waitForService(ARM_IK_NAME);
ROS_INFO("Services ready");
// action_client = new TrajClient("joint_trajectory_action", true);
action_client = new TrajClient("follow_joint_trajectory", true);
while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0)))
{
ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
}
// service = node.advertiseService("execute_cartesian_ik_trajectory",
// &IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);
const std::string input_topic = "execute_cartesian_ik_trajectory";
joint_states_sub = nh->subscribe( "joint_states", 1,
&IKTrajectoryExecutor::set_current_joint_angles, this);
ros::Duration(1.0).sleep();
trajectory_sub = nh->subscribe( input_topic, 1,
&IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);
ros::spinOnce();
goal.trajectory.joint_names.push_back("shoulder_pan_joint");
goal.trajectory.joint_names.push_back("shoulder_lift_joint");
goal.trajectory.joint_names.push_back("elbow_joint");
goal.trajectory.joint_names.push_back("wrist_1_joint");
goal.trajectory.joint_names.push_back("wrist_2_joint");
goal.trajectory.joint_names.push_back("wrist_3_joint");
}
//! Returns the current state of the action
actionlib::SimpleClientGoalState getState()
{
return traj_client_->getState();
}
//! Sends the command to start a given trajectory
void startTrajectory(arm_navigation_msgs::MoveArmGoal& goal)
{
traj_client_->sendGoal(goal);
}
actionlib::SimpleClientGoalState getArmState()
{
return traj_client_elbow_flex_->getState();
}
//send a desired joint trajectory to the joint trajectory action
//and wait for it to finish
bool execute_joint_trajectory(std::vector<double *> joint_trajectory)
{
int i, j;
int trajectorylength = joint_trajectory.size();
//get the current joint angles
double current_angles[6];
get_current_joint_angles(current_angles);
//fill the goal message with the desired joint trajectory
goal.trajectory.points.resize(trajectorylength+1);
//set the first trajectory point to the current position
goal.trajectory.points[0].positions.resize(6);
goal.trajectory.points[0].velocities.resize(6);
for(j=0; j<6; j++)
{
goal.trajectory.points[0].positions[j] = current_angles[j];
goal.trajectory.points[0].velocities[j] = 0.0;
}
//make the first trajectory point start 0.25 seconds from when we run
goal.trajectory.points[0].time_from_start = ros::Duration(0.25);
//fill in the rest of the trajectory
double time_from_start = 0.25;
for(i=0; i<trajectorylength; i++)
{
goal.trajectory.points[i+1].positions.resize(6);
goal.trajectory.points[i+1].velocities.resize(6);
//fill in the joint positions (velocities of 0 mean that the arm
//will try to stop briefly at each waypoint)
for(j=0; j<6; j++)
{
goal.trajectory.points[i+1].positions[j] = joint_trajectory[i][j];
goal.trajectory.points[i+1].velocities[j] = 0.0;
}
//compute a desired time for this trajectory point using a max
//joint velocity
double max_joint_move = 0;
for(j=0; j<6; j++)
{
double joint_move = fabs(goal.trajectory.points[i+1].positions[j]
- goal.trajectory.points[i].positions[j]);
if(joint_move > max_joint_move) max_joint_move = joint_move;
}
double seconds = max_joint_move/MAX_JOINT_VEL;
ROS_INFO("max_joint_move: %0.3f, seconds: %0.3f", max_joint_move, seconds);
time_from_start += seconds;
goal.trajectory.points[i+1].time_from_start =
ros::Duration(time_from_start);
}
//when to start the trajectory
goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.25);
ROS_INFO("Sending goal to joint_trajectory_action");
action_client->sendGoal(goal);
action_client->waitForResult();
//get the current joint angles for debugging
get_current_joint_angles(current_angles);
// ROS_INFO("joint angles after trajectory: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f\n",current_angles[0],current_angles[1],current_angles[2],current_angles[3],current_angles[4],current_angles[5]);
if(action_client->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
ROS_INFO("Hooray, the arm finished the trajectory!");
return 1;
}
ROS_INFO("The arm failed to execute the trajectory.");
return 0;
}
//! Sends the command to start a given trajectory
void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal)
{
// When to start the trajectory: 1s from now
goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
traj_client_->sendGoal(goal);
}
