机器人已知关节位置的位置、速度、加速度求力矩

flyfish

力矩的wiki解释：
在物理学里，作用力促使物体绕着转动轴或支点转动的趋向，称为力矩（torque），也就是扭转的力。转动力矩又称为转矩。力矩能够使物体改变其旋转运动。推挤或拖拉涉及到作用力 ，而扭转则涉及到力矩。

简略地说，力矩是一种施加于好像螺栓或飞轮一类的物体的扭转力。例如，用扳手的开口箝紧螺栓或螺帽，然后转动扳手，这动作会产生力矩来转动螺栓或螺帽。

根据国际单位制，力矩的单位是牛顿.米。

KDL
使用的是KDL(The Kinematics and Dynamics Library )
Orocos Kinematics and Dynamics　Smarter in motion!

主要用到里面的运动学和动力学求解:各种通用的正逆运动学算法（Kinematic and Dynamic Solvers: various generic forward and inverse kinematic algorithms）

KDL首先要加载URDF文件

URDF
URDF（Unified Robot Description Format，统一机器人描述格式）是ROS中一个非常重要的机器人模型描述格式，ROS同时也提供URDF文件的C++解析器，可以解析URDF文件中使用XML格式描述的机器人模型。
有的机器人描述文件使用的是Xacro,虽然有如下好处
By now, if you’re following all these steps at home with your own robot design, you might be sick of doing all sorts of math to get very simple robot descriptions to parse correctly. Fortunately, you can use the xacro package to make your life simpler. It does three things that are very helpful.
Constants
Simple Math
Macros
但是KDL是不能直接读的，需要转成URDF文件，命令如下
rosrun xacro xacro model.xacro > model.urdf

求力矩主要是使用一个函数KDL::ChainIdSolver_RNE递归牛顿欧拉法算法
Recursive Newton Euler Algorithm　RNE的缩写　或者说是牛顿－欧拉递推动力学算法
<<机器人学导论>>中的描述
函数说明

KDL::ChainIdSolver_RNE::ChainIdSolver_RNE   (   const Chain &   chain,
        Vector      grav 
    )       

Constructor for the solver, it will allocate all the necessary memory

Parameters
    chain   The kinematic chain to calculate the inverse dynamics for, an internal copy will be made.
    grav    The gravity vector to use during the calculation. 

Member Function Documentation
int KDL::ChainIdSolver_RNE::CartToJnt   (   const JntArray &    q,
        const JntArray &    q_dot,
        const JntArray &    q_dotdot,
        const Wrenches &    f_ext,
        JntArray &      torques 
    )       
    virtual

Function to calculate from Cartesian forces to joint torques. Input parameters;

Parameters
    q   The current joint positions
    q_dot   The current joint velocities
    q_dotdot    The current joint accelerations
    f_ext   The external forces (no gravity) on the segments Output parameters:
    torques the resulting torques for the joints 

KDL的例子
Forward kinematic chain example

// Copyright  (C)  2007  Francois Cauwe <francois at cauwe dot org>

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include <stdio.h>
#include <iostream>

using namespace KDL;


int main( int argc, char** argv )
{
    //Definition of a kinematic chain & add segments to the chain
    KDL::Chain chain;
    chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
    chain.addSegment(Segment(Joint(Joint::RotZ)));
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120))));
    chain.addSegment(Segment(Joint(Joint::RotZ)));

    // Create solver based on kinematic chain
    ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);

    // Create joint array
    unsigned int nj = chain.getNrOfJoints();
    KDL::JntArray jointpositions = JntArray(nj);

    // Assign some values to the joint positions
    for(unsigned int i=0;i<nj;i++){
        float myinput;
        printf ("Enter the position of joint %i: ",i);
        scanf ("%e",&myinput);
        jointpositions(i)=(double)myinput;
    }

    // Create the frame that will contain the results
    KDL::Frame cartpos;    

    // Calculate forward position kinematics
    bool kinematics_status;
    kinematics_status = fksolver.JntToCart(jointpositions,cartpos);
    if(kinematics_status>=0){
        std::cout << cartpos <<std::endl;
        printf("%s \n","Succes, thanks KDL!");
    }else{
        printf("%s \n","Error: could not calculate forward kinematics :(");
    }
}

Inverse position example

//Creation of the chain:
KDL::Chain chain;
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
chain.addSegment(Segment(Joint(Joint::RotZ)));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120))));
chain.addSegment(Segment(Joint(Joint::RotZ)));

//Creation of the solvers:
ChainFkSolverPos_recursive fksolver1(chain1);//Forward position solver
ChainIkSolverVel_pinv iksolver1v(chain1);//Inverse velocity solver
ChainIkSolverPos_NR iksolver1(chain1,fksolver1,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6

//Creation of jntarrays:
JntArray q(chain.getNrOfJoints());
JntArray q_init(chain.getNrOfJoints());

//Set destination frame
Frame F_dest=...;

int ret = iksolverpos.CartToJnt(q_init,F_dest,q);

MoveIt封装了KDL，可以直接使用Sachin Chitta写的代码
dynamics_solver.h

/* Author: Sachin Chitta */

#ifndef MOVEIT_DYNAMICS_SOLVER_DYNAMICS_SOLVER_
#define MOVEIT_DYNAMICS_SOLVER_DYNAMICS_SOLVER_

// KDL
#include <kdl/chain.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>

#include <moveit/robot_state/robot_state.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Wrench.h>

#include <memory>

/** \brief This namespace includes the dynamics_solver library */
namespace dynamics_solver
{
MOVEIT_CLASS_FORWARD(DynamicsSolver);

/**
 * This solver currently computes the required torques given a
 * joint configuration, velocities, accelerations and external wrenches
 * acting on the links of a robot
 */
class DynamicsSolver
{
public:
  /**
   * @brief Initialize the dynamics solver
   * @param urdf_model The urdf model for the robot
   * @param srdf_model The srdf model for the robot
   * @param group_name The name of the group to compute stuff for
   * @return False if initialization failed
   */
  DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name,
                 const geometry_msgs::Vector3& gravity_vector);

  /**
   * @brief Get the torques (the order of all input and output is the same
   * as the order of joints for this group in the RobotModel)
   * @param joint_angles The joint angles (desired joint configuration)
   * this must have size = number of joints in the group
   * @param joint_velocities The desired joint velocities
   * this must have size = number of joints in the group
   * @param joint_accelerations The desired joint accelerations
   * this must have size = number of joints in the group
   * @param wrenches External wrenches acting on the links of the robot
   * this must have size = number of links in the group
   * @param torques Computed set of torques are filled in here
   * this must have size = number of joints in the group
   * @return False if any of the input vectors are of the wrong size
   */
  bool getTorques(const std::vector<double>& joint_angles, const std::vector<double>& joint_velocities,
                  const std::vector<double>& joint_accelerations, const std::vector<geometry_msgs::Wrench>& wrenches,
                  std::vector<double>& torques) const;

  /**
   * @brief Get the maximum payload for this group (in kg). Payload is
   * the weight that this group can hold when the weight is attached to the origin
   * of the last link of this group. (The order of joint_angles vector is the same
   * as the order of joints for this group in the RobotModel)
   * @param joint_angles The joint angles (desired joint configuration)
   * this must have size = number of joints in the group
   * @param payload The computed maximum payload
   * @param joint_saturated The first saturated joint and the maximum payload
   * @return False if the input set of joint angles is of the wrong size
   */
  bool getMaxPayload(const std::vector<double>& joint_angles, double& payload, unsigned int& joint_saturated) const;

  /**
   * @brief Get torques corresponding to a particular payload value.  Payload is
   * the weight that this group can hold when the weight is attached to the origin
   * of the last link of this group.
   * @param joint_angles The joint angles (desired joint configuration)
   * this must have size = number of joints in the group
   * @param payload The payload for which to compute torques (in kg)
   * @param joint_torques The resulting joint torques
   * @return False if the input vectors are of the wrong size
   */
  bool getPayloadTorques(const std::vector<double>& joint_angles, double payload,
                         std::vector<double>& joint_torques) const;

  /**
   * @brief Get maximum torques for this group
   * @return Vector of max torques
   */
  const std::vector<double>& getMaxTorques() const;

  /**
   * @brief Get the kinematic model
   * @return kinematic model
   */
  const robot_model::RobotModelConstPtr& getRobotModel() const
  {
    return robot_model_;
  }

  const robot_model::JointModelGroup* getGroup() const
  {
    return joint_model_group_;
  }

private:
  std::shared_ptr<KDL::ChainIdSolver_RNE> chain_id_solver_;  // KDL chain inverse dynamics
  KDL::Chain kdl_chain_;                                     // KDL chain

  robot_model::RobotModelConstPtr robot_model_;
  const robot_model::JointModelGroup* joint_model_group_;

  robot_state::RobotStatePtr state_;  // robot state

  std::string base_name_, tip_name_;        // base name, tip name
  unsigned int num_joints_, num_segments_;  // number of joints in group, number of segments in group
  std::vector<double> max_torques_;         // vector of max torques

  double gravity_;  // Norm of the gravity vector passed in initialize()
};
}
#endif

dynamics_solver.cpp

/* Author: Sachin Chitta */

#include <moveit/dynamics_solver/dynamics_solver.h>

// KDL
#include <kdl/jntarray.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/tree.hpp>

namespace dynamics_solver
{
namespace
{
inline geometry_msgs::Vector3 transformVector(const Eigen::Affine3d& transform, const geometry_msgs::Vector3& vector)
{
  Eigen::Vector3d p;
  p = Eigen::Vector3d(vector.x, vector.y, vector.z);
  p = transform.rotation() * p;

  geometry_msgs::Vector3 result;
  result.x = p.x();
  result.y = p.y();
  result.z = p.z();

  return result;
}
}

DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name,
                               const geometry_msgs::Vector3& gravity_vector)
{
  robot_model_ = robot_model;
  joint_model_group_ = robot_model_->getJointModelGroup(group_name);
  if (!joint_model_group_)
    return;

  if (!joint_model_group_->isChain())
  {
    ROS_ERROR_NAMED("dynamics_solver", "Group '%s' is not a chain. Will not initialize dynamics solver",
                    group_name.c_str());
    joint_model_group_ = nullptr;
    return;
  }

  if (!joint_model_group_->getMimicJointModels().empty())
  {
    ROS_ERROR_NAMED("dynamics_solver", "Group '%s' has a mimic joint. Will not initialize dynamics solver",
                    group_name.c_str());
    joint_model_group_ = nullptr;
    return;
  }

  const robot_model::JointModel* joint = joint_model_group_->getJointRoots()[0];
  if (!joint->getParentLinkModel())
  {
    ROS_ERROR_NAMED("dynamics_solver", "Group '%s' does not have a parent link", group_name.c_str());
    joint_model_group_ = nullptr;
    return;
  }

  base_name_ = joint->getParentLinkModel()->getName();

  tip_name_ = joint_model_group_->getLinkModelNames().back();
  ROS_DEBUG_NAMED("dynamics_solver", "Base name: '%s', Tip name: '%s'", base_name_.c_str(), tip_name_.c_str());

  const urdf::ModelInterfaceSharedPtr urdf_model = robot_model_->getURDF();
  const srdf::ModelConstSharedPtr srdf_model = robot_model_->getSRDF();
  KDL::Tree tree;

  if (!kdl_parser::treeFromUrdfModel(*urdf_model, tree))
  {
    ROS_ERROR_NAMED("dynamics_solver", "Could not initialize tree object");
    joint_model_group_ = nullptr;
    return;
  }
  if (!tree.getChain(base_name_, tip_name_, kdl_chain_))
  {
    ROS_ERROR_NAMED("dynamics_solver", "Could not initialize chain object");
    joint_model_group_ = nullptr;
    return;
  }
  num_joints_ = kdl_chain_.getNrOfJoints();
  num_segments_ = kdl_chain_.getNrOfSegments();

  state_.reset(new robot_state::RobotState(robot_model_));
  state_->setToDefaultValues();

  const std::vector<std::string>& joint_model_names = joint_model_group_->getJointModelNames();
  for (std::size_t i = 0; i < joint_model_names.size(); ++i)
  {
    const urdf::Joint* ujoint = urdf_model->getJoint(joint_model_names[i]).get();
    if (ujoint && ujoint->limits)
      max_torques_.push_back(ujoint->limits->effort);
    else
      max_torques_.push_back(0.0);
  }

  KDL::Vector gravity(gravity_vector.x, gravity_vector.y,
                      gravity_vector.z);  // \todo Not sure if KDL expects the negative of this (Sachin)
  gravity_ = gravity.Norm();
  ROS_DEBUG_NAMED("dynamics_solver", "Gravity norm set to %f", gravity_);

  chain_id_solver_.reset(new KDL::ChainIdSolver_RNE(kdl_chain_, gravity));
}

bool DynamicsSolver::getTorques(const std::vector<double>& joint_angles, const std::vector<double>& joint_velocities,
                                const std::vector<double>& joint_accelerations,
                                const std::vector<geometry_msgs::Wrench>& wrenches, std::vector<double>& torques) const
{
  if (!joint_model_group_)
  {
    ROS_DEBUG_NAMED("dynamics_solver", "Did not construct DynamicsSolver object properly. "
                                       "Check error logs.");
    return false;
  }
  if (joint_angles.size() != num_joints_)
  {
    ROS_ERROR_NAMED("dynamics_solver", "Joint angles vector should be size %d", num_joints_);
    return false;
  }
  if (joint_velocities.size() != num_joints_)
  {
    ROS_ERROR_NAMED("dynamics_solver", "Joint velocities vector should be size %d", num_joints_);
    return false;
  }
  if (joint_accelerations.size() != num_joints_)
  {
    ROS_ERROR_NAMED("dynamics_solver", "Joint accelerations vector should be size %d", num_joints_);
    return false;
  }
  if (wrenches.size() != num_segments_)
  {
    ROS_ERROR_NAMED("dynamics_solver", "Wrenches vector should be size %d", num_segments_);
    return false;
  }
  if (torques.size() != num_joints_)
  {
    ROS_ERROR_NAMED("dynamics_solver", "Torques vector should be size %d", num_joints_);
    return false;
  }

  KDL::JntArray kdl_angles(num_joints_), kdl_velocities(num_joints_), kdl_accelerations(num_joints_),
      kdl_torques(num_joints_);
  KDL::Wrenches kdl_wrenches(num_segments_);

  for (unsigned int i = 0; i < num_joints_; ++i)
  {
    kdl_angles(i) = joint_angles[i];
    kdl_velocities(i) = joint_velocities[i];
    kdl_accelerations(i) = joint_accelerations[i];
  }

  for (unsigned int i = 0; i < num_segments_; ++i)
  {
    kdl_wrenches[i](0) = wrenches[i].force.x;
    kdl_wrenches[i](1) = wrenches[i].force.y;
    kdl_wrenches[i](2) = wrenches[i].force.z;

    kdl_wrenches[i](3) = wrenches[i].torque.x;
    kdl_wrenches[i](4) = wrenches[i].torque.y;
    kdl_wrenches[i](5) = wrenches[i].torque.z;
  }

  if (chain_id_solver_->CartToJnt(kdl_angles, kdl_velocities, kdl_accelerations, kdl_wrenches, kdl_torques) < 0)
  {
    ROS_ERROR_NAMED("dynamics_solver", "Something went wrong computing torques");
    return false;
  }

  for (unsigned int i = 0; i < num_joints_; ++i)
    torques[i] = kdl_torques(i);

  return true;
}

bool DynamicsSolver::getMaxPayload(const std::vector<double>& joint_angles, double& payload,
                                   unsigned int& joint_saturated) const
{
  if (!joint_model_group_)
  {
    ROS_DEBUG_NAMED("dynamics_solver", "Did not construct DynamicsSolver object properly. "
                                       "Check error logs.");
    return false;
  }
  if (joint_angles.size() != num_joints_)
  {
    ROS_ERROR_NAMED("dynamics_solver", "Joint angles vector should be size %d", num_joints_);
    return false;
  }
  std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
  std::vector<double> torques(num_joints_, 0.0), zero_torques(num_joints_, 0.0);

  std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
  if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, zero_torques))
    return false;

  for (unsigned int i = 0; i < num_joints_; ++i)
  {
    if (fabs(zero_torques[i]) >= max_torques_[i])
    {
      payload = 0.0;
      joint_saturated = i;
      return true;
    }
  }

  state_->setJointGroupPositions(joint_model_group_, joint_angles);
  const Eigen::Affine3d& base_frame = state_->getFrameTransform(base_name_);
  const Eigen::Affine3d& tip_frame = state_->getFrameTransform(tip_name_);
  Eigen::Affine3d transform = tip_frame.inverse() * base_frame;
  wrenches.back().force.z = 1.0;
  wrenches.back().force = transformVector(transform, wrenches.back().force);
  wrenches.back().torque = transformVector(transform, wrenches.back().torque);

  ROS_DEBUG_NAMED("dynamics_solver", "New wrench (local frame): %f %f %f", wrenches.back().force.x,
                  wrenches.back().force.y, wrenches.back().force.z);

  if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, torques))
    return false;

  double min_payload = std::numeric_limits<double>::max();
  for (unsigned int i = 0; i < num_joints_; ++i)
  {
    double payload_joint = std::max<double>((max_torques_[i] - zero_torques[i]) / (torques[i] - zero_torques[i]),
                                            (-max_torques_[i] - zero_torques[i]) /
                                                (torques[i] - zero_torques[i]));  // because we set the payload to 1.0
    ROS_DEBUG_NAMED("dynamics_solver", "Joint: %d, Actual Torque: %f, Max Allowed: %f, Gravity: %f", i, torques[i],
                    max_torques_[i], zero_torques[i]);
    ROS_DEBUG_NAMED("dynamics_solver", "Joint: %d, Payload Allowed (N): %f", i, payload_joint);
    if (payload_joint < min_payload)
    {
      min_payload = payload_joint;
      joint_saturated = i;
    }
  }
  payload = min_payload / gravity_;
  ROS_DEBUG_NAMED("dynamics_solver", "Max payload (kg): %f", payload);
  return true;
}

bool DynamicsSolver::getPayloadTorques(const std::vector<double>& joint_angles, double payload,
                                       std::vector<double>& joint_torques) const
{
  if (!joint_model_group_)
  {
    ROS_DEBUG_NAMED("dynamics_solver", "Did not construct DynamicsSolver object properly. "
                                       "Check error logs.");
    return false;
  }
  if (joint_angles.size() != num_joints_)
  {
    ROS_ERROR_NAMED("dynamics_solver", "Joint angles vector should be size %d", num_joints_);
    return false;
  }
  if (joint_torques.size() != num_joints_)
  {
    ROS_ERROR_NAMED("dynamics_solver", "Joint torques vector should be size %d", num_joints_);
    return false;
  }
  std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
  std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
  state_->setJointGroupPositions(joint_model_group_, joint_angles);

  const Eigen::Affine3d& base_frame = state_->getFrameTransform(base_name_);
  const Eigen::Affine3d& tip_frame = state_->getFrameTransform(tip_name_);
  Eigen::Affine3d transform = tip_frame.inverse() * base_frame;
  wrenches.back().force.z = payload * gravity_;
  wrenches.back().force = transformVector(transform, wrenches.back().force);
  wrenches.back().torque = transformVector(transform, wrenches.back().torque);

  ROS_DEBUG_NAMED("dynamics_solver", "New wrench (local frame): %f %f %f", wrenches.back().force.x,
                  wrenches.back().force.y, wrenches.back().force.z);

  return getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, joint_torques);
}

const std::vector<double>& DynamicsSolver::getMaxTorques() const
{
  return max_torques_;
}

}  // end of namespace dynamics_solver