机器人已知关节位置的位置、速度、加速度求力矩
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
来源:CSDN
作者:flyfish1986
链接:https://blog.csdn.net/flyfish1986/article/details/82630338