kinton_arm_tag_tracker_alg.cpp

Go to the documentation of this file.

#include "kinton_arm_tag_tracker_alg.h"

// using namespace KDL;
// using namespace Eigen;

KintonArmTagTrackerAlgorithm::KintonArmTagTrackerAlgorithm(void)
{
}

KintonArmTagTrackerAlgorithm::~KintonArmTagTrackerAlgorithm(void)
{
}

void KintonArmTagTrackerAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// KintonArmTagTrackerAlgorithm Public API

void KintonArmTagTrackerAlgorithm::tag_tracker(const MatrixXd& cam_vel,
                                                                                           const Chain& kdl_chain,
                                                                                           const double& dt,
                                                                                           MatrixXd& q)
{
  this->lock();

  // Get forward kinematics
  MatrixXd fkine = this->kinematics_.fkinematics(kdl_chain,q);


  // Get actual arm jacobian
  MatrixXd jacobian = this->kinematics_.jacobian(kdl_chain,q);

  // Get joint velocities from expected end effector velocities
  MatrixXd vel_joints = this->kinematics_.jointvel_from_endeffvel(cam_vel,jacobian);

  MatrixXd inc = vel_joints * dt*180/3.1416;

  // Get new joint positions
  q = q + inc;

  for (int i = 0; i < q.rows(); ++i)
  {
    if (q(i)>180) q(i)=180;
    if (q(i)<0) q(i)=0;
  }

  // MatrixXd Ttarget(4,4);
  // Ttarget = MatrixXd::Zero(4,4);
  // Ttarget.block(0,0,3,3) = MatrixXd::Identity(3,3);
  // Ttarget(0,3)=-0.1;
  // Ttarget(1,3)=0;
  // Ttarget(2,3)=0.1;
  // Ttarget(3,3)=1;

  // MatrixXd q_pos=q;
  // MatrixXd q_new=MatrixXd::Zero(5,1); 
  // bool ik = this->kinematics_.ikinematics(kdl_chain,Ttarget,q_pos,q_new);

  // if (ik) q=q_new;

  // ROS_INFO_STREAM(q);

  this->unlock();
}