kinton_arm_task_priority_control_alg.cpp

Go to the documentation of this file.

#include "kinton_arm_task_priority_control_alg.h"

using namespace KDL;
using namespace Eigen;

KintonArmTaskPriorityControlAlgorithm::KintonArmTaskPriorityControlAlgorithm(void)
{
}

KintonArmTaskPriorityControlAlgorithm::~KintonArmTaskPriorityControlAlgorithm(void)
{
}

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

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

// KintonArmTaskPriorityControlAlgorithm Public API

void KintonArmTaskPriorityControlAlgorithm::task_priority_ctrl(const MatrixXd& cam_vel,const bool& arm_unina,
                                                               const bool& arm_catec,const bool& enable_sec_task,
                                                               const Matrix4d& Tcam_in_tip,const Matrix4d& Ttag_in_cam,
                                                               const MatrixXd& lambda_robot,
                                                               const MatrixXd& gain_sec_task,
                                                               const MatrixXd& q_rollpitch,const MatrixXd& v_rollpitch,
                                                               const Chain& kdl_chain,const vector<arm_joint> joint_info,
                                                               const double& dt, MatrixXd& q,
                                                               MatrixXd& vel_quad)
{
  this->lock();
    quadarm_task_priority_ctrl.quadarm_task_priority_ctrl(cam_vel,arm_unina,arm_catec,enable_sec_task,Tcam_in_tip,Ttag_in_cam,lambda_robot,gain_sec_task,q_rollpitch,v_rollpitch,kdl_chain,joint_info,dt,q,vel_quad);
  this->unlock();
}