kinton_arm_task_priority_control_alg_node.cpp

Go to the documentation of this file.

#include "kinton_arm_task_priority_control_alg_node.h"

using namespace KDL;
using namespace Eigen;
using namespace std;

KintonArmTaskPriorityControlAlgNode::KintonArmTaskPriorityControlAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<KintonArmTaskPriorityControlAlgorithm>()
{
  //init class attributes if necessary
  this->loop_rate_ = 100;//in [Hz]

  this->init_ = true;
  //init_arm();

  this->time_ = ros::Time::now();

  this->cam_vel_ = MatrixXd::Zero(6,1);
  this->v_rollpitch_ = MatrixXd::Zero(2,1);

  this->num_joints_ = 0;
  this->cam_vel_ok_ = false;
  this->v_rollpitch_ = MatrixXd::Zero(2,1);
  this->q_rollpitch_ = MatrixXd::Zero(2,1);
  this->gain_sec_task_ = MatrixXd::Zero(3,1);

  this->public_node_handle_.param<bool>("arm_unina", this->arm_unina_, "true" );
  this->public_node_handle_.param<bool>("arm_catec", this->arm_catec_, "false" );
  this->public_node_handle_.param<bool>("enable_sec_task", this->enable_sec_task_, "true" );

  ros::NodeHandle nh;
 
  string tip_name, root_name, optical_name;
  string xml_string;
  Tree tree;   
  string urdf_xml,full_urdf_xml;
  nh.param("urdf_xml",urdf_xml,string("robot_description"));
  nh.searchParam(urdf_xml,full_urdf_xml);      
  ROS_DEBUG("Reading xml file from parameter server\n");
  if (!nh.getParam(full_urdf_xml, xml_string)){
    ROS_ERROR("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
  }
  if (!nh.getParam("root_name", root_name)){
    ROS_ERROR("No root name found on parameter server");
  }
  if (!nh.getParam("tip_name", tip_name)){
    ROS_ERROR("No tip name found on parameter server");
  }

  // KDL Parser to create a KDL chain
  if (!kdl_parser::treeFromString(xml_string, tree)){
    ROS_ERROR("Could not initialize tree object");
  }
  if (!tree.getChain(root_name, tip_name, this->kdl_chain_)){
    ROS_ERROR("Could not initialize kdl_chain object");
  }

  // Get Joints Information
  urdf::Model robot_model;
  if (!robot_model.initString(xml_string)) {
    ROS_ERROR("Could not initialize robot model");
  }
  if (!readJoints(robot_model,root_name,tip_name)) {
    ROS_ERROR("Could not read information about the joints");
  }

  if (!nh.getParam("optical_frame_id", optical_name)){
    ROS_ERROR("No camera optical frame name found on parameter server");
  }

  // Get Homogenous transform
  this->Tcam_in_tip_ = get_HT_cam_in_tip(optical_name,tip_name);

  // [init publishers]
  this->joints_pub_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("arm_joints_states", 10);
  this->quad_vel_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 10);
  
  // [init subscribers]
  this->cam_vel_subscriber_ = this->public_node_handle_.subscribe("cam_vel", 100, &KintonArmTaskPriorityControlAlgNode::cam_vel_callback, this);
  this->odom_subscriber_ = this->public_node_handle_.subscribe("odom", 100, &KintonArmTaskPriorityControlAlgNode::odom_callback, this);
  this->input_tag_subscriber_ = this->public_node_handle_.subscribe("input_tag", 10, &KintonArmTaskPriorityControlAlgNode::input_tag_callback, this);

  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void KintonArmTaskPriorityControlAlgNode::mainNodeThread(void)
{
  
  // Initialize arm and time
  if (this->init_)
  {
    this->time_last_=this->time_;
    init_arm();
    // Initialize cmd_vel output
    this->vel_ = MatrixXd::Zero(6+this->num_joints_,1); 
    this->init_=false;
  }

  this->dt_=(this->time_-this->time_last_).toSec(); 


  if (this->cam_vel_ok_ && this->activate_){

    this->alg_.lock();          
    this->task_priority_ctrl_.task_priority_ctrl(this->cam_vel_,this->arm_unina_,
                                                 this->arm_catec_,this->enable_sec_task_,
                                                 this->Tcam_in_tip_,this->Ttag_in_cam_,
                                                 this->lambda_robot_,this->gain_sec_task_,
                                                 this->q_rollpitch_,this->v_rollpitch_,
                                                 this->kdl_chain_,this->joint_info_,this->dt_,this->q_,this->vel_);
    this->alg_.unlock();   
  }
  else 
  {
    this->init_ = true; 
  }

  // [fill msg structures]

  // fill arm joints message
  for (int jj=0; jj<this->num_joints_; ++jj){
    this->JointState_msg_.position[jj]=this->q_(6+jj,0);
    this->JointState_msg_.velocity[jj]=this->vel_(6+jj);
    this->JointState_msg_.effort[jj]=1;
  }    

  // fill quadrotor velocity msg 
  this->Twist_msg_.linear.x = this->vel_(0,0);
  this->Twist_msg_.linear.y = this->vel_(1,0);
  this->Twist_msg_.linear.z = this->vel_(2,0); 
  this->Twist_msg_.angular.x = this->vel_(3,0);  
  this->Twist_msg_.angular.y = this->vel_(4,0);  
  this->Twist_msg_.angular.z = this->vel_(5,0); 

  // adding an offset to the thrust to compensate the arm weight
  if (this->arm_unina_){
    this->Twist_msg_.linear.z= this->Twist_msg_.linear.z + 0.15;
  }
  else if (this->arm_catec_){
    this->Twist_msg_.linear.z= this->Twist_msg_.linear.z + 0;
  }
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
  this->quad_vel_publisher_.publish(this->Twist_msg_);
  this->joints_pub_publisher_.publish(this->JointState_msg_); 
}

/*  [subscriber callbacks] */
void KintonArmTaskPriorityControlAlgNode::cam_vel_callback(const geometry_msgs::TwistWithCovariance::ConstPtr& msg) 
{ 
  //ROS_INFO("KintonArmTaskPriorityControlAlgNode::twist_cov_callback: New Message Received"); 

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

  if (msg->covariance[0]<1000 && this->activate_)
  {
    this->cam_vel_(0,0) = msg->twist.linear.x;
    this->cam_vel_(1,0) = msg->twist.linear.y;
    this->cam_vel_(2,0) = msg->twist.linear.z;
    this->cam_vel_(3,0) = msg->twist.angular.x;
    this->cam_vel_(4,0) = msg->twist.angular.y;
    this->cam_vel_(5,0) = msg->twist.angular.z;

    this->cam_vel_ok_ = true;
  }
  else
  {
    this->cam_vel_ = MatrixXd::Zero(6,1);
    this->cam_vel_ok_ = false;
  } 

  //unlock previously blocked shared variables 
  this->cam_vel_mutex_.exit(); 
}

void KintonArmTaskPriorityControlAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg) 
{ 
  //ROS_INFO("KintonVsControlAlgNode::odom_callback: New Message Received"); 

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

  // In the case of a underactuation
  // Extract the Non controllable DOF velocities
  this->v_rollpitch_ = MatrixXd::Zero(2,1);
  this->q_rollpitch_ = MatrixXd::Zero(2,1);

  if (msg->twist.covariance[0]<100)
  {

    if (!isnan(msg->twist.twist.angular.x) && !isnan(msg->twist.twist.angular.y)){
      this->v_rollpitch_(0,0) = msg->twist.twist.angular.x;
      this->v_rollpitch_(1,0) = msg->twist.twist.angular.y; 
    }

    if (!isnan(msg->pose.pose.orientation.x)){

      // Convert quaternion to RPY.
      tf::Quaternion qt;
      double quad_roll, quad_pitch, quad_yaw;
      tf::quaternionMsgToTF(msg->pose.pose.orientation, qt);
      tf::Matrix3x3(qt).getRPY(quad_roll, quad_pitch, quad_yaw);

      this->q_rollpitch_(0,0) = quad_roll;
      this->q_rollpitch_(1,0) = quad_pitch; 
    }

  }   

  this->v_rollpitch_ = MatrixXd::Zero(2,1);

  //unlock previously blocked shared variables 
  this->odom_mutex_.exit(); 
}

void KintonArmTaskPriorityControlAlgNode::input_tag_callback(const ar_pose::ARMarkers::ConstPtr& msg) 
{ 
  this->input_tag_mutex_.enter();

  this->time_last_ = this->time_;
  
  
  if (msg->markers.empty()){
    this->Ttag_in_cam_ = Matrix4d::Zero();
    this->Ttag_in_cam_.block(0,0,3,3) = Matrix3d::Identity();
    this->Ttag_in_cam_(3,3) = 1.0;
    this->time_ = ros::Time::now();
    this->init_ = true;
    ROS_INFO("No tag detected");
  }

  if ((!msg->markers.empty()) && ((!std::isnan(msg->markers[0].pose.pose.orientation.x) && std::isfinite(msg->markers[0].pose.pose.orientation.x)) || (!std::isnan(msg->markers[1].pose.pose.orientation.x) && std::isfinite(msg->markers[1].pose.pose.orientation.x)))){
        
    this->time_ = msg->markers.back().header.stamp;
    this->pose_ = msg->markers.back().pose.pose;
    // Convert pose from marker (quaternion to RPY).
    tf::Transform tf_pose;
    
    double x,y,z,yaw,pitch,roll;
    tf::poseMsgToTF(this->pose_, tf_pose);
    tf_pose.getBasis().getRPY(roll,pitch,yaw);
    
    Matrix3d Rot;
    // euler convention zyx  
    Rot = AngleAxisd(yaw, Vector3d::UnitZ()) \
      * AngleAxisd(pitch, Vector3d::UnitY()) \
      * AngleAxisd(roll, Vector3d::UnitX());
    
    x = this->pose_.position.x;
    y = this->pose_.position.y;
    z = this->pose_.position.z;
    
    this->Ttag_in_cam_=Matrix4d::Zero();
    if (!std::isnan(Rot(0,0)))  this->Ttag_in_cam_.block(0,0,3,3)=Rot;
     
    this->Ttag_in_cam_.block(0,3,3,1) << x, y, z;
    this->Ttag_in_cam_.row(3) << 0, 0, 0, 1;
  }  
  //unlock previously blocked shared variables 
  this->input_tag_mutex_.exit();
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

Matrix4d KintonArmTaskPriorityControlAlgNode::getTransform(const string& frame1, const string& frame2)
{
//Get fixed transforms between the IMU data and the robot frame
  tf::TransformListener listener;
  tf::StampedTransform transform;

  try {
    //listener.waitForTransform(frame1,frame2, ros::Time(0));
    listener.lookupTransform(frame1,frame2, ros::Time(0), transform);
  } 
  catch (tf::TransformException ex) {
    //ROS_ERROR("End-effector in Inertial frame Transform error: %s",ex.what());
  }

  double roll,pitch,yaw;
  tf::Quaternion q =  transform.getRotation();
  tf::Matrix3x3(q).getRPY(roll,pitch,yaw); 

  Matrix3d Rot;
  Rot = AngleAxisd(yaw, Vector3d::UnitZ()) \
      * AngleAxisd(pitch, Vector3d::UnitY()) \
      * AngleAxisd(roll, Vector3d::UnitX());
  Matrix4d T = Matrix4d::Zero();

  T.block(0,0,3,3) = Rot;
  T(0,3) = transform.getOrigin().x();
  T(1,3) = transform.getOrigin().y();
  T(2,3) = transform.getOrigin().z();
  T(3,3) = 1.0;  

  //ROS_INFO("x: %f y: %f z: %f roll: %f, pitch: %f, yaw: %f",transform.getOrigin().x(),transform.getOrigin().y(),transform.getOrigin().z(),roll,pitch,yaw);
  return T;
}

void KintonArmTaskPriorityControlAlgNode::init_arm()
{

  this->q_ = MatrixXd::Zero(6+this->num_joints_,1);

  //UNINA arm
  if (this->arm_unina_ && !this->arm_catec_){

    // frontal marker
    //this->q_ << 0, 0, 2.5, 0, 0, 0, 0, -0.5, 2, 3, 0;
    //BO this->q_ << 0, 0, 2.5, 0, 0, 0, 0, 0.2, 1.7, 2, 0;
    //this->q_ << 0, 0, 2.5, 0, 0, 0, 0, 0.2, 1.7, 2, 0;
    //this->q_ << 0, 0, 2.5, 0, 0, 0, 0, 1.5, 0, 2, 0;
    //this->q_ << 0, 0, 2.5, 0, 0, 0, 0, -0.5, 1.7, 1, 0;
    // extended
    this->q_ << 0, 0, 2.5, 0, 0, 0, 0, 0, 3.1415, 3.1415, 0;
  }
  //CATEC arm
  else if (this->arm_catec_ && !this->arm_unina_){
     
    // this->q_ << 0, 0, 2.5, 0, 0, 0, 0, 3.14159/2, 0, 0, 0, 0;
    this->q_ << 0, 0, 2.5, 0, 0, 0, 0, 2, -2, 0, 1.5707, 0;
  }
  else if ((this->arm_unina_ && this->arm_catec_) || (!this->arm_unina_ && !this->arm_catec_)){
      ROS_ERROR("Wrong arms configuration");
  }

  this->JointState_msg_.header.stamp=ros::Time::now();
  this->JointState_msg_.header.frame_id="";
  this->JointState_msg_.name.resize(this->num_joints_);
  this->JointState_msg_.position.resize(this->num_joints_);
  this->JointState_msg_.velocity.resize(this->num_joints_);
  this->JointState_msg_.effort.resize(this->num_joints_);
  for (int ii = 0; ii < this->num_joints_; ++ii)
  {
    string quad_str="quadrotor::";
    stringstream joint_name;
    joint_name << quad_str << this->joint_info_.at(ii).link_child;
    this->JointState_msg_.name[ii]=joint_name.str();
  }
}

void KintonArmTaskPriorityControlAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  this->activate_=config.sim_activate;
  double lambda_quadrotor=config.lambda_quadrotor;
  double lambda_arm=config.lambda_arm;
  this->enable_sec_task_ = config.enable_sec_task;
  // this->gain_sec_task_(0,0) = config.gain_sec_task_stabil;
  this->gain_sec_task_(1,0) = config.gain_sec_task_CoG;
  this->gain_sec_task_(2,0) = config.gain_sec_task_jntlim;

  this->lambda_robot_ = MatrixXd::Zero(4+this->num_joints_,1);

  for (int i = 0; i < 4; ++i)
  {
    this->lambda_robot_(i,0)=lambda_quadrotor;
  }
  for (int i = 0; i < this->num_joints_; ++i)
  {
    this->lambda_robot_(4+i,0)=lambda_arm;
  }

  this->alg_.unlock();
}

void KintonArmTaskPriorityControlAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<KintonArmTaskPriorityControlAlgNode>(argc, argv, "kinton_arm_task_priority_control_alg_node");
}

bool KintonArmTaskPriorityControlAlgNode::readJoints(urdf::Model &robot_model,const string& root_name,const string& tip_name) {
  
  int num_joints = 0;
  // get joint maxs and mins
  boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
  boost::shared_ptr<const urdf::Joint> joint;

  while (link && link->name != root_name) {
    joint = robot_model.getJoint(link->parent_joint->name);
    if (!joint) {
      ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
      return false;
    }
    if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
      //ROS_INFO( "adding joint: [%s]", joint->name.c_str() );
      num_joints++;
    }
    link = robot_model.getLink(link->getParent()->name);
  }

  this->num_joints_=num_joints;
  this->joint_info_.resize(num_joints);
  
  link = robot_model.getLink(tip_name);
  int ii = 0;
  while (link && ii < num_joints) {
    joint = robot_model.getJoint(link->parent_joint->name);
    if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
      //ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() );

      float lower, upper;
      if ( joint->type != urdf::Joint::CONTINUOUS ) {
        lower = joint->limits->lower;
        upper = joint->limits->upper;
      } else {
        lower = -M_PI;
        upper = M_PI;
      }
      int index = num_joints - ii -1;
      this->joint_info_.at(index).joint_min = lower;
      this->joint_info_.at(index).joint_max = upper;
      this->joint_info_.at(index).link_child = link->name;
      this->joint_info_.at(index).link_parent = link->getParent()->name;
      ii++;
    }
    link = robot_model.getLink(link->getParent()->name);
  }
  return true;
}

Matrix4d KintonArmTaskPriorityControlAlgNode::get_HT_cam_in_tip(const string& cam_frame, const string& tip_frame)
{
  Matrix4d Tcam_in_tip = Matrix4d::Zero();

  tf::TransformListener listener;
  tf::StampedTransform transform;
  try{ 
      listener.waitForTransform(tip_frame, cam_frame, ros::Time(0), ros::Duration(1.0));
      listener.lookupTransform(tip_frame, cam_frame, ros::Time(0), transform);
      }
      catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
      }
  
  double x,y,z,roll,pitch,yaw;
  transform.getBasis().getRPY(roll, pitch, yaw);
  
  x=transform.getOrigin().x();
  y=transform.getOrigin().y();
  z=transform.getOrigin().z();

  Matrix3d Rot;
  // euler convention zyx    
  Rot = AngleAxisd(yaw, Vector3d::UnitZ()) \
       * AngleAxisd(pitch, Vector3d::UnitY()) \
     * AngleAxisd(roll, Vector3d::UnitX());
 
  Tcam_in_tip.block(0,0,3,3) = Rot;
  Tcam_in_tip(0,3) = x;
  Tcam_in_tip(1,3) = y;
  Tcam_in_tip(2,3) = z;
  Tcam_in_tip(3,3) = 1;

  return Tcam_in_tip;
}