C++ (Cpp) toKDL示例

示例#1

文件： cds_wrapper.cpp 项目： brahayam/cram_seds

const KDL::JntArray& CdsWrapper::update(const KDL::JntArray& q, double dt,
    KDL::Frame& des_pose)
{
  assert(q.rows() == arm_.get_dof());

  KDL::Frame pose = arm_.get_pos_fk(q);

  cds_controller_.setCurrentEEPose(toMathLib(pose));

  des_pose = toKDL(cds_controller_.getNextEEPose());

  des_pose.M = arm_.get_pos_fk(q).M;
//  return arm_.get_vel_ik(q, toKDL(cds_controller_.getNextEEPose()), dt);
  return arm_.get_vel_ik(q, des_pose, dt);
}

示例#2

文件： cds_cartesian_wrapper.cpp 项目： RoboHow/cram_seds

KDL::Frame CdsCartesianWrapper::update(const KDL::Frame& current_pose)
{
  cds_controller_.setCurrentEEPose(toMathLib(current_pose));
  return toKDL(cds_controller_.getNextEEPose());
}