darwin_robot_driver_node.cpp

Go to the documentation of this file.

#include "darwin_robot_driver_node.h"
#include <math.h>

DarwinRobotDriverNode::DarwinRobotDriverNode(ros::NodeHandle &nh) : 
  iri_base_driver::IriBaseNodeDriver<DarwinRobotDriver>(nh),
  tracking_head_aserver_(public_node_handle_, "tracking_head"),
  move_right_arm_joints_aserver_(public_node_handle_, "move_right_arm_joints"),
  move_left_arm_joints_aserver_(public_node_handle_, "move_left_arm_joints"),
  move_joints_aserver_(public_node_handle_, "move_joints")
{
  //init class attributes if necessary
  this->loop_rate_ = 10;//in [Hz]

  // [init publishers]  
  this->adc_channels_publisher_ = this->public_node_handle_.advertise<std_msgs::Float64MultiArray>("adc_channels", 100);
  this->inertial_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Imu>("inertial", 100);
  this->joint_states_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("joint_states", 100);
  
  // [init subscribers]
  this->head_target_subscriber_ = this->public_node_handle_.subscribe("head_target", 100, &DarwinRobotDriverNode::head_target_callback, this);
  this->execute_action_subscriber_ = this->public_node_handle_.subscribe("execute_action", 100, &DarwinRobotDriverNode::execute_action_callback, this);
  this->cmd_vel_subscriber_ = this->public_node_handle_.subscribe("cmd_vel", 100, &DarwinRobotDriverNode::cmd_vel_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  tracking_head_aserver_.registerStartCallback(boost::bind(&DarwinRobotDriverNode::tracking_headStartCallback, this, _1)); 
  tracking_head_aserver_.registerStopCallback(boost::bind(&DarwinRobotDriverNode::tracking_headStopCallback, this)); 
  tracking_head_aserver_.registerIsFinishedCallback(boost::bind(&DarwinRobotDriverNode::tracking_headIsFinishedCallback, this)); 
  tracking_head_aserver_.registerHasSucceedCallback(boost::bind(&DarwinRobotDriverNode::tracking_headHasSucceedCallback, this)); 
  tracking_head_aserver_.registerGetResultCallback(boost::bind(&DarwinRobotDriverNode::tracking_headGetResultCallback, this, _1)); 
  tracking_head_aserver_.registerGetFeedbackCallback(boost::bind(&DarwinRobotDriverNode::tracking_headGetFeedbackCallback, this, _1)); 
  tracking_head_aserver_.start();
  move_right_arm_joints_aserver_.registerStartCallback(boost::bind(&DarwinRobotDriverNode::move_right_arm_jointsStartCallback, this, _1)); 
  move_right_arm_joints_aserver_.registerStopCallback(boost::bind(&DarwinRobotDriverNode::move_right_arm_jointsStopCallback, this)); 
  move_right_arm_joints_aserver_.registerIsFinishedCallback(boost::bind(&DarwinRobotDriverNode::move_right_arm_jointsIsFinishedCallback, this)); 
  move_right_arm_joints_aserver_.registerHasSucceedCallback(boost::bind(&DarwinRobotDriverNode::move_right_arm_jointsHasSucceedCallback, this)); 
  move_right_arm_joints_aserver_.registerGetResultCallback(boost::bind(&DarwinRobotDriverNode::move_right_arm_jointsGetResultCallback, this, _1)); 
  move_right_arm_joints_aserver_.registerGetFeedbackCallback(boost::bind(&DarwinRobotDriverNode::move_right_arm_jointsGetFeedbackCallback, this, _1)); 
  move_right_arm_joints_aserver_.start();
  move_left_arm_joints_aserver_.registerStartCallback(boost::bind(&DarwinRobotDriverNode::move_left_arm_jointsStartCallback, this, _1)); 
  move_left_arm_joints_aserver_.registerStopCallback(boost::bind(&DarwinRobotDriverNode::move_left_arm_jointsStopCallback, this)); 
  move_left_arm_joints_aserver_.registerIsFinishedCallback(boost::bind(&DarwinRobotDriverNode::move_left_arm_jointsIsFinishedCallback, this)); 
  move_left_arm_joints_aserver_.registerHasSucceedCallback(boost::bind(&DarwinRobotDriverNode::move_left_arm_jointsHasSucceedCallback, this)); 
  move_left_arm_joints_aserver_.registerGetResultCallback(boost::bind(&DarwinRobotDriverNode::move_left_arm_jointsGetResultCallback, this, _1)); 
  move_left_arm_joints_aserver_.registerGetFeedbackCallback(boost::bind(&DarwinRobotDriverNode::move_left_arm_jointsGetFeedbackCallback, this, _1)); 
  move_left_arm_joints_aserver_.start();
  move_joints_aserver_.registerStartCallback(boost::bind(&DarwinRobotDriverNode::move_jointsStartCallback, this, _1)); 
  move_joints_aserver_.registerStopCallback(boost::bind(&DarwinRobotDriverNode::move_jointsStopCallback, this)); 
  move_joints_aserver_.registerIsFinishedCallback(boost::bind(&DarwinRobotDriverNode::move_jointsIsFinishedCallback, this)); 
  move_joints_aserver_.registerHasSucceedCallback(boost::bind(&DarwinRobotDriverNode::move_jointsHasSucceedCallback, this)); 
  move_joints_aserver_.registerGetResultCallback(boost::bind(&DarwinRobotDriverNode::move_jointsGetResultCallback, this, _1)); 
  move_joints_aserver_.registerGetFeedbackCallback(boost::bind(&DarwinRobotDriverNode::move_jointsGetFeedbackCallback, this, _1)); 

  this->left_arm_target_id="";
  this->right_arm_target_id="";
  this->general_target_id="";
  
  // [init action clients]  
}

void DarwinRobotDriverNode::mainNodeThread(void)
{
  static bool first=true;
  int i=0;
  double gyr_x=0.0,gyr_y=0.0,gyr_z=0.0;
  double accel_x=0.0,accel_y=0.0,accel_z=0.0;  
  double voltage[15];

  //lock access to driver if necessary
  this->driver_.lock();
  if(first)
  {
    first=false;
    tracking_head_aserver_.start();
    move_joints_aserver_.start();
    move_right_arm_joints_aserver_.start();
    move_left_arm_joints_aserver_.start();
  }

  if(this->driver_.getState()==DarwinRobotDriver::RUNNING)
  {
    // [fill msg Header if necessary]
    this->JointState_msg_.header.stamp = ros::Time::now();
    this->JointState_msg_.header.frame_id = "MP_BODY";
  
    this->Imu_msg_.header.stamp = ros::Time::now();
    this->Imu_msg_.header.frame_id = "MP_BODY";
    // [fill msg structures]      
  //this->Float64MultiArray_msg_.data = my_var;
    this->JointState_msg_.name.resize(JointData::GetNumServos());
    this->JointState_msg_.velocity.resize(JointData::GetNumServos());
    this->JointState_msg_.effort.resize(JointData::GetNumServos());
    // get the current angle values
    this->JointState_msg_.position=this->driver_.get_current_angles();
    this->JointState_msg_.name[1]="j_shoulder_l"; 
    this->JointState_msg_.name[0]="j_shoulder_r"; 
    this->JointState_msg_.name[3]="j_high_arm_l"; 
    this->JointState_msg_.name[2]="j_high_arm_r"; 
    this->JointState_msg_.name[5]="j_low_arm_l"; 
    this->JointState_msg_.name[4]="j_low_arm_r"; 
    this->JointState_msg_.name[7]="j_pelvis_l"; 
    this->JointState_msg_.name[6]="j_pelvis_r"; 
    this->JointState_msg_.name[9]="j_thigh1_l"; 
    this->JointState_msg_.name[8]="j_thigh1_r"; 
    this->JointState_msg_.name[11]="j_thigh2_l"; 
    this->JointState_msg_.name[10]="j_thigh2_r"; 
    this->JointState_msg_.name[13]="j_tibia_l"; 
    this->JointState_msg_.name[12]="j_tibia_r"; 
    this->JointState_msg_.name[15]="j_ankle1_l"; 
    this->JointState_msg_.name[14]="j_ankle1_r"; 
    this->JointState_msg_.name[17]="j_ankle2_l"; 
    this->JointState_msg_.name[16]="j_ankle2_r"; 
    this->JointState_msg_.name[18]="j_pan"; 
    this->JointState_msg_.name[19]="j_tilt"; 
    this->JointState_msg_.name[20]="j_wrist_r"; 
    this->JointState_msg_.name[21]="j_gripper_r"; 
    this->JointState_msg_.name[22]="j_wrist_l"; 
    this->JointState_msg_.name[23]="j_gripper_l"; 

    // get the inertial sensor data
    this->driver_.get_gyroscope(&gyr_x,&gyr_y,&gyr_z);
    this->driver_.get_accelerometer(&accel_x,&accel_y,&accel_z);
    this->driver_.get_analog_channel(CHANNEL1,&voltage[0]);
    this->driver_.get_analog_channel(CHANNEL2,&voltage[1]);
    this->driver_.get_analog_channel(CHANNEL3,&voltage[2]);
    this->driver_.get_analog_channel(CHANNEL4,&voltage[3]);
    this->driver_.get_analog_channel(CHANNEL5,&voltage[4]);
    this->driver_.get_analog_channel(CHANNEL6,&voltage[5]);
    this->driver_.get_analog_channel(CHANNEL7,&voltage[6]);
    this->driver_.get_analog_channel(CHANNEL8,&voltage[7]);
    this->driver_.get_analog_channel(CHANNEL9,&voltage[8]);
    this->driver_.get_analog_channel(CHANNEL10,&voltage[9]);
    this->driver_.get_analog_channel(CHANNEL11,&voltage[10]);
    this->driver_.get_analog_channel(CHANNEL12,&voltage[11]);
    this->driver_.get_analog_channel(CHANNEL13,&voltage[12]);
    this->driver_.get_analog_channel(CHANNEL14,&voltage[13]);
    this->driver_.get_analog_channel(CHANNEL15,&voltage[14]);
    this->Imu_msg_.orientation_covariance[0] = -1;// no orientation information
    this->Imu_msg_.angular_velocity.x=gyr_x;
    this->Imu_msg_.angular_velocity.y=gyr_y;
    this->Imu_msg_.angular_velocity.z=gyr_z;
    this->Float64MultiArray_msg_.data.resize(15);
    for(i=0;i<15;i++)    
      this->Float64MultiArray_msg_.data[i]=voltage[i];
    // angular velocity covariance
    this->Imu_msg_.angular_velocity_covariance[0]=1;
    this->Imu_msg_.angular_velocity_covariance[4]=1;
    this->Imu_msg_.angular_velocity_covariance[8]=1;
    this->Imu_msg_.linear_acceleration.x=accel_x;
    this->Imu_msg_.linear_acceleration.y=accel_y;
    this->Imu_msg_.linear_acceleration.z=accel_z;
    // linear acceleration covariance
    this->Imu_msg_.linear_acceleration_covariance[0]=1;
    this->Imu_msg_.linear_acceleration_covariance[4]=1;
    this->Imu_msg_.linear_acceleration_covariance[8]=1;
      

    // [fill srv structure and make request to the server]
  
    // [fill action structure and make request to the action server]

    // [publish messages]          
    this->adc_channels_publisher_.publish(this->Float64MultiArray_msg_);
    this->inertial_publisher_.publish(this->Imu_msg_);
    this->joint_states_publisher_.publish(this->JointState_msg_);
  }
  //unlock access to driver if previously blocked
  this->driver_.unlock();
}

/*  [subscriber callbacks] */
void DarwinRobotDriverNode::head_target_callback(const std_msgs::Float64MultiArray::ConstPtr& msg) 
{ 
  
  ROS_INFO("DarwinRobotDriverNode::head_target_callback: New Head Target"); 
  
  //use appropiate mutex to shared variables if necessary 
  this->driver_.lock(); 
  //this->head_target_mutex_.enter();
  this->head_target_=(*msg);  
  this->head_target_.data.resize(2);
  this->driver_.set_tracking_target(head_target_.data[0],head_target_.data[1]);
  //std::cout << msg->data << std::endl; 

  //unlock previously blocked shared variables 
  this->driver_.unlock(); 
  //this->head_target_mutex_.exit(); 
}
void DarwinRobotDriverNode::execute_action_callback(const std_msgs::Int32::ConstPtr& msg) 
{ 
//  ROS_INFO("DarwinNodeDriverNode::execute_action_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  this->driver_.lock(); 
  //this->execute_action_mutex_.enter(); 
  this->driver_.start_action(msg->data);


  //unlock previously blocked shared variables 
  this->driver_.unlock(); 
  //this->execute_action_mutex_.exit(); 
}
void DarwinRobotDriverNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) 
{ 
  double period=0.0,x_vel=0.0,r_vel=0.0,y_vel=0.0;

  //use appropiate mutex to shared variables if necessary 
  this->driver_.lock(); 
  //this->cmd_vel_mutex_.enter(); 

//  if(this->driver_.action_is_running())
//    this->driver_.stop_action();
  // convert the twist into DARWIN motion parameters
  // msg->linear.z must be 0
  // msg->angular.x must be 0
  // msg->angular.y must be 0 
  // ge the period time of the motion (two steps in each period)
  period=this->driver_.get_walking_param(PERIOD_TIME)/1000.0;// in seconds
  // compute the linear motion parameters to achieve the desired linear speed
  x_vel=(msg->linear.x*period*1000.0)/2.0;// in mm per step
  y_vel=(msg->linear.y*period*1000.0)/2.0;// in mm per step
  // compute the angular motion parameters to achieve the desired angular speed
  r_vel=(msg->angular.z*period*180.0)/(2.0*3.14159);// in degrees per step
  ROS_INFO("Platform: vt: (%f,%f), vr: %f",x_vel,y_vel,r_vel); 
  // start or change motion
  if(fabs(x_vel) <= 0.01 && fabs(y_vel) <= 0.01 && fabs(r_vel) <= 0.01)
  {
    ROS_INFO("Stop walking");
    this->driver_.stop_walking();
  }
  else
  {
    if(this->driver_.is_walking())
      this->driver_.tune_walking(x_vel,y_vel,r_vel);
    else
      this->driver_.start_walking(x_vel,y_vel,r_vel);
  }
  
  //unlock previously blocked shared variables 
  this->driver_.unlock(); 
  //this->cmd_vel_mutex_.exit(); 
}

/*  [service callbacks] */

/*  [action callbacks] */
void DarwinRobotDriverNode::tracking_headStartCallback(const iri_darwin_robot::tracking_headGoalConstPtr& goal)
{ 
  TPID pan_pid,tilt_pid;

  driver_.lock(); 
  try{
    //init PID parameters
    pan_pid.kp=0.05;//0.1;
    pan_pid.ki=0.001;
    pan_pid.kd=0.0001;
    tilt_pid.kp=0.03;//0.05;
    tilt_pid.ki=0.001;
    tilt_pid.kd=0.0001;
    this->driver_.set_tracking_pid(10,&pan_pid,&tilt_pid);
    //check goal
  
    //execute goal
    this->driver_.start_head_tracking();  
  }catch(CDarwinException &e){
    this->tracking_head_aserver_.setAborted();
  }
  driver_.unlock(); 
} 

void DarwinRobotDriverNode::tracking_headStopCallback(void) 
{ 
  driver_.lock(); 
    //stop action 
    this->driver_.stop_head_tracking();    
  driver_.unlock(); 
} 

bool DarwinRobotDriverNode::tracking_headIsFinishedCallback(void) 
{ 
  bool ret = false; 

  driver_.lock(); 
    //if action has finish for any reason 
    //ret = true; 
  driver_.unlock(); 

  return ret; 
} 

bool DarwinRobotDriverNode::tracking_headHasSucceedCallback(void) 
{ 
  bool ret = false; 

  driver_.lock(); 
  //if goal was accomplished
     ret = true; 
  driver_.unlock(); 

  return ret; 
} 

void DarwinRobotDriverNode::tracking_headGetResultCallback(iri_darwin_robot::tracking_headResultPtr& result) 
{ 
  driver_.lock(); 
    //update result data to be sent to client 
    //result->data = data; 
  driver_.unlock(); 
} 

void DarwinRobotDriverNode::tracking_headGetFeedbackCallback(iri_darwin_robot::tracking_headFeedbackPtr& feedback) 
{ 
  driver_.lock(); 
    //keep track of feedback 
    this->driver_.get_head_position(&head_target_.data[0],&head_target_.data[1]);
    feedback->current_pan=head_target_.data[0];
    feedback->current_tilt=head_target_.data[1];
    //std::cout << "head target: " << feedback->current_pan << "," << feedback->current_tilt << std::endl;
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
  driver_.unlock(); 
}
void DarwinRobotDriverNode::move_right_arm_jointsStartCallback(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal)
{ 
  std::vector<double> angles,speeds,accels;
  std::vector<int> servos;
  unsigned int i=0;

  driver_.lock();
  try{
    //check goal 
    servos.resize(goal->trajectory.joint_names.size());
    angles.resize(goal->trajectory.joint_names.size());
    speeds.resize(goal->trajectory.joint_names.size());
    accels.resize(goal->trajectory.joint_names.size());
    for(i=0;i<goal->trajectory.joint_names.size();i++)
    {
      if(goal->trajectory.joint_names[i]=="j_shoulder_r")
        servos[i]=1;
      else if(goal->trajectory.joint_names[i]=="j_high_arm_r")
        servos[i]=3;
      else if(goal->trajectory.joint_names[i]=="j_low_arm_r")
        servos[i]=5;
      else if(goal->trajectory.joint_names[i]=="j_wrist_r")
        servos[i]=21;
      else
        throw CDarwinException(_HERE_,"Invalid servo identifier.");
      angles[i]=goal->trajectory.points[0].positions[i];
      speeds[i]=goal->trajectory.points[0].velocities[i];
      accels[i]=goal->trajectory.points[0].accelerations[i];
    }
    this->right_arm_target_id=this->driver_.move_joints(servos,angles,speeds,accels);
  }catch(CDarwinException &e){
    this->move_right_arm_joints_aserver_.setAborted();
  }
    //execute goal 
  driver_.unlock(); 
} 

void DarwinRobotDriverNode::move_right_arm_jointsStopCallback(void) 
{ 
  driver_.lock(); 
    //stop action 
    this->driver_.stop_joints(this->right_arm_target_id);
  driver_.unlock(); 
} 

bool DarwinRobotDriverNode::move_right_arm_jointsIsFinishedCallback(void) 
{ 
  bool ret = false; 

  driver_.lock(); 
    //if action has finish for any reason 
    ret=!this->driver_.joints_are_moving(this->right_arm_target_id);
    //ret = true; 
  driver_.unlock(); 

  return ret; 
} 

bool DarwinRobotDriverNode::move_right_arm_jointsHasSucceedCallback(void) 
{ 
  bool ret = false; 

  driver_.lock(); 
    //if goal was accomplished 
    ret = true; 
  driver_.unlock(); 

  return ret; 
} 

void DarwinRobotDriverNode::move_right_arm_jointsGetResultCallback(control_msgs::FollowJointTrajectoryResultPtr& result) 
{ 
  driver_.lock(); 
    //update result data to be sent to client 
    //result->data = data; 
  driver_.unlock(); 
} 

void DarwinRobotDriverNode::move_right_arm_jointsGetFeedbackCallback(control_msgs::FollowJointTrajectoryFeedbackPtr& feedback) 
{ 
  driver_.lock(); 
    //keep track of feedback 
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
  driver_.unlock(); 
}
void DarwinRobotDriverNode::move_left_arm_jointsStartCallback(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal)
{ 
  std::vector<double> angles,speeds,accels;
  std::vector<int> servos;
  unsigned int i=0;

  driver_.lock();
  try{
    //check goal 
    servos.resize(goal->trajectory.joint_names.size());
    angles.resize(goal->trajectory.joint_names.size());
    speeds.resize(goal->trajectory.joint_names.size());
    accels.resize(goal->trajectory.joint_names.size());
    for(i=0;i<goal->trajectory.joint_names.size();i++)
    {
      if(goal->trajectory.joint_names[i]=="j_shoulder_l")
        servos[i]=2;
      else if(goal->trajectory.joint_names[i]=="j_high_arm_l")
        servos[i]=4;
      else if(goal->trajectory.joint_names[i]=="j_low_arm_l")
        servos[i]=6;
      else if(goal->trajectory.joint_names[i]=="j_wrist_l")
        servos[i]=23;
      else
        throw CDarwinException(_HERE_,"Invalid servo identifier");
      angles[i]=goal->trajectory.points[0].positions[i];
      speeds[i]=goal->trajectory.points[0].velocities[i];
      accels[i]=goal->trajectory.points[0].accelerations[i];
    }
    this->left_arm_target_id=this->driver_.move_joints(servos,angles,speeds,accels);
  }catch(CDarwinException &e){
    this->move_left_arm_joints_aserver_.setAborted();
  } 
    //execute goal 
  driver_.unlock(); 
} 

void DarwinRobotDriverNode::move_left_arm_jointsStopCallback(void) 
{ 
  driver_.lock(); 
    //stop action 
    this->driver_.stop_joints(this->left_arm_target_id);
  driver_.unlock(); 
} 

bool DarwinRobotDriverNode::move_left_arm_jointsIsFinishedCallback(void) 
{ 
  bool ret = false; 

  driver_.lock(); 
    //if action has finish for any reason 
    ret=!this->driver_.joints_are_moving(this->left_arm_target_id);
    //ret = true; 
  driver_.unlock(); 

  return ret; 
} 

bool DarwinRobotDriverNode::move_left_arm_jointsHasSucceedCallback(void) 
{ 
  bool ret = false; 

  driver_.lock(); 
    //if goal was accomplished 
    ret = true; 
  driver_.unlock(); 

  return ret; 
} 

void DarwinRobotDriverNode::move_left_arm_jointsGetResultCallback(control_msgs::FollowJointTrajectoryResultPtr& result) 
{ 
  driver_.lock(); 
    //update result data to be sent to client 
    //result->data = data; 
  driver_.unlock(); 
} 

void DarwinRobotDriverNode::move_left_arm_jointsGetFeedbackCallback(control_msgs::FollowJointTrajectoryFeedbackPtr& feedback) 
{ 
  driver_.lock(); 
    //keep track of feedback 
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
  driver_.unlock(); 
}
void DarwinRobotDriverNode::move_jointsStartCallback(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal)
{ 
  std::vector<double> angles,speeds,accels;
  std::vector<int> servos;
  unsigned int i=0;

  driver_.lock(); 
  try{
    //check goal 
    servos.resize(goal->trajectory.joint_names.size());
    angles.resize(goal->trajectory.joint_names.size());
    speeds.resize(goal->trajectory.joint_names.size());
    accels.resize(goal->trajectory.joint_names.size());
    for(i=0;i<goal->trajectory.joint_names.size();i++)
    {
      std::cout << goal->trajectory.joint_names[i] << " -> ";
      if(goal->trajectory.joint_names[i]=="j_shoulder_l")
        servos[i]=2;
      else if(goal->trajectory.joint_names[i]=="j_shoulder_r")
        servos[i]=1;
      else if(goal->trajectory.joint_names[i]=="j_high_arm_l")
        servos[i]=4;
      else if(goal->trajectory.joint_names[i]=="j_high_arm_r")
        servos[i]=3;
      else if(goal->trajectory.joint_names[i]=="j_low_arm_l")
        servos[i]=6;
      else if(goal->trajectory.joint_names[i]=="j_low_arm_r")
        servos[i]=5;
      else if(goal->trajectory.joint_names[i]=="j_pelvis_l")
        servos[i]=8;
      else if(goal->trajectory.joint_names[i]=="j_pelvis_r")
        servos[i]=7;
      else if(goal->trajectory.joint_names[i]=="j_thigh1_l")
        servos[i]=10;
      else if(goal->trajectory.joint_names[i]=="j_thigh1_r")
        servos[i]=9;
      else if(goal->trajectory.joint_names[i]=="j_thigh2_l")
        servos[i]=12;
      else if(goal->trajectory.joint_names[i]=="j_thigh2_r")
        servos[i]=11;
      else if(goal->trajectory.joint_names[i]=="j_tibia_l")
        servos[i]=14;
      else if(goal->trajectory.joint_names[i]=="j_tibia_r")
        servos[i]=13;
      else if(goal->trajectory.joint_names[i]=="j_ankle1_l")
        servos[i]=16;
      else if(goal->trajectory.joint_names[i]=="j_ankle1_r")
        servos[i]=15;
      else if(goal->trajectory.joint_names[i]=="j_ankle2_l")
        servos[i]=18;
      else if(goal->trajectory.joint_names[i]=="j_ankle2_r")
        servos[i]=17;
      else if(goal->trajectory.joint_names[i]=="j_pan")
        servos[i]=19;
      else if(goal->trajectory.joint_names[i]=="j_tilt")
        servos[i]=20;
      else if(goal->trajectory.joint_names[i]=="j_wrist_r")
        servos[i]=21;
      else if(goal->trajectory.joint_names[i]=="j_gripper_r")
        servos[i]=22;
      else if(goal->trajectory.joint_names[i]=="j_wrist_l")
        servos[i]=23;
      else if(goal->trajectory.joint_names[i]=="j_gripper_l")
        servos[i]=24;
      else
        throw CDarwinException(_HERE_,"Invalid servo identifier.");
      angles[i]=goal->trajectory.points[0].positions[i];
      speeds[i]=goal->trajectory.points[0].velocities[i];
      accels[i]=goal->trajectory.points[0].accelerations[i];
    }
    this->general_target_id=this->driver_.move_joints(servos,angles,speeds,accels);
  }catch(CDarwinException &e){
    this->move_left_arm_joints_aserver_.setAborted();
  }
    //execute goal 
  driver_.unlock(); 
} 

void DarwinRobotDriverNode::move_jointsStopCallback(void) 
{ 
  driver_.lock(); 
    //stop action 
    this->driver_.stop_joints(this->general_target_id);
  driver_.unlock(); 
} 

bool DarwinRobotDriverNode::move_jointsIsFinishedCallback(void) 
{ 
  bool ret = false; 

  driver_.lock(); 
    //if action has finish for any reason 
    ret=!this->driver_.joints_are_moving(this->general_target_id);
    //ret = true; 
  driver_.unlock(); 

  return ret; 
} 

bool DarwinRobotDriverNode::move_jointsHasSucceedCallback(void) 
{ 
  bool ret = false; 

  driver_.lock(); 
    //if goal was accomplished 
    ret = true; 
  driver_.unlock(); 

  return ret; 
} 

void DarwinRobotDriverNode::move_jointsGetResultCallback(control_msgs::FollowJointTrajectoryResultPtr& result) 
{ 
  driver_.lock(); 
    //update result data to be sent to client 
    //result->data = data; 
  driver_.unlock(); 
} 

void DarwinRobotDriverNode::move_jointsGetFeedbackCallback(control_msgs::FollowJointTrajectoryFeedbackPtr& feedback) 
{ 
  driver_.lock(); 
    //keep track of feedback   
  driver_.unlock(); 
}

/*  [action requests] */

void DarwinRobotDriverNode::postNodeOpenHook(void)
{
}

void DarwinRobotDriverNode::addNodeDiagnostics(void)
{
}

void DarwinRobotDriverNode::addNodeOpenedTests(void)
{
}

void DarwinRobotDriverNode::addNodeStoppedTests(void)
{
}

void DarwinRobotDriverNode::addNodeRunningTests(void)
{
}

void DarwinRobotDriverNode::reconfigureNodeHook(int level)
{
}

DarwinRobotDriverNode::~DarwinRobotDriverNode(void)
{
  // [free dynamic memory]
}

/* main function */
int main(int argc,char *argv[])
{
  return driver_base::main<DarwinRobotDriverNode>(argc, argv, "darwin_robot_driver_node");
}