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");

  }

	// 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");
    }
  }

  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");

  }

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

 //! 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);
   }     
 }

 //! 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);
 }