00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/dynamics_solver/dynamics_solver.h>
00038
00039
00040 #include <kdl/jntarray.hpp>
00041 #include <kdl_parser/kdl_parser.hpp>
00042 #include <kdl/chainiksolverpos_nr_jl.hpp>
00043 #include <kdl/chainiksolvervel_pinv.hpp>
00044 #include <kdl/chainfksolverpos_recursive.hpp>
00045 #include <kdl/chainjnttojacsolver.hpp>
00046 #include <kdl/tree.hpp>
00047
00048 namespace dynamics_solver
00049 {
00050
00051 namespace
00052 {
00053 inline geometry_msgs::Vector3 transformVector(const Eigen::Affine3d &transform,
00054 const geometry_msgs::Vector3 &vector)
00055 {
00056 Eigen::Vector3d p;
00057 p = Eigen::Vector3d(vector.x, vector.y, vector.z);
00058 p = transform.rotation()*p;
00059
00060 geometry_msgs::Vector3 result;
00061 result.x = p.x();
00062 result.y = p.y();
00063 result.z = p.z();
00064
00065 return result;
00066 }
00067 }
00068
00069 DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr &robot_model,
00070 const std::string &group_name,
00071 const geometry_msgs::Vector3 &gravity_vector)
00072 {
00073 robot_model_ = robot_model;
00074 joint_model_group_ = robot_model_->getJointModelGroup(group_name);
00075 if (!joint_model_group_)
00076 return;
00077
00078 if (!joint_model_group_->isChain())
00079 {
00080 logError("moveit.dynamics_solver: Group '%s' is not a chain. Will not initialize dynamics solver", group_name.c_str());
00081 joint_model_group_ = NULL;
00082 return;
00083 }
00084
00085 if (joint_model_group_->getMimicJointModels().size() > 0)
00086 {
00087 logError("moveit.dynamics_solver: Group '%s' has a mimic joint. Will not initialize dynamics solver", group_name.c_str());
00088 joint_model_group_ = NULL;
00089 return;
00090 }
00091
00092 const robot_model::JointModel* joint = joint_model_group_->getJointRoots()[0];
00093 if (!joint->getParentLinkModel())
00094 {
00095 logError("moveit.dynamics_solver: Group '%s' does not have a parent link", group_name.c_str());
00096 joint_model_group_ = NULL;
00097 return;
00098 }
00099
00100 base_name_ = joint->getParentLinkModel()->getName();
00101
00102 tip_name_ = joint_model_group_->getLinkModelNames().back();
00103 logDebug("moveit.dynamics_solver: Base name: '%s', Tip name: '%s'", base_name_.c_str(), tip_name_.c_str());
00104
00105 const boost::shared_ptr<const urdf::ModelInterface> urdf_model = robot_model_->getURDF();
00106 const boost::shared_ptr<const srdf::Model> srdf_model = robot_model_->getSRDF();
00107 KDL::Tree tree;
00108
00109 if (!kdl_parser::treeFromUrdfModel(*urdf_model, tree))
00110 {
00111 logError("moveit.dynamics_solver: Could not initialize tree object");
00112 joint_model_group_ = NULL;
00113 return;
00114 }
00115 if (!tree.getChain(base_name_, tip_name_, kdl_chain_))
00116 {
00117 logError("moveit.dynamics_solver: Could not initialize chain object");
00118 joint_model_group_ = NULL;
00119 return;
00120 }
00121 num_joints_ = kdl_chain_.getNrOfJoints();
00122 num_segments_ = kdl_chain_.getNrOfSegments();
00123
00124 state_.reset(new robot_state::RobotState(robot_model_));
00125 state_->setToDefaultValues();
00126
00127 const std::vector<std::string> &joint_model_names = joint_model_group_->getJointModelNames();
00128 for (std::size_t i = 0; i < joint_model_names.size(); ++i)
00129 {
00130 const urdf::Joint* ujoint = urdf_model->getJoint(joint_model_names[i]).get();
00131 if (ujoint && ujoint->limits)
00132 max_torques_.push_back(ujoint->limits->effort);
00133 else
00134 max_torques_.push_back(0.0);
00135 }
00136
00137 KDL::Vector gravity(gravity_vector.x, gravity_vector.y, gravity_vector.z);
00138 gravity_ = gravity.Norm();
00139 logDebug("moveit.dynamics_solver: Gravity norm set to %f", gravity_);
00140
00141 chain_id_solver_.reset(new KDL::ChainIdSolver_RNE(kdl_chain_, gravity));
00142 }
00143
00144 bool DynamicsSolver::getTorques(const std::vector<double> &joint_angles,
00145 const std::vector<double> &joint_velocities,
00146 const std::vector<double> &joint_accelerations,
00147 const std::vector<geometry_msgs::Wrench> &wrenches,
00148 std::vector<double> &torques) const
00149 {
00150 if (!joint_model_group_)
00151 {
00152 logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. Check error logs.");
00153 return false;
00154 }
00155 if (joint_angles.size() != num_joints_)
00156 {
00157 logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
00158 return false;
00159 }
00160 if (joint_velocities.size() != num_joints_)
00161 {
00162 logError("moveit.dynamics_solver: Joint velocities vector should be size %d", num_joints_);
00163 return false;
00164 }
00165 if (joint_accelerations.size() != num_joints_)
00166 {
00167 logError("moveit.dynamics_solver: Joint accelerations vector should be size %d", num_joints_);
00168 return false;
00169 }
00170 if (wrenches.size() != num_segments_)
00171 {
00172 logError("moveit.dynamics_solver: Wrenches vector should be size %d", num_segments_);
00173 return false;
00174 }
00175 if (torques.size() != num_joints_)
00176 {
00177 logError("moveit.dynamics_solver: Torques vector should be size %d", num_joints_);
00178 return false;
00179 }
00180
00181 KDL::JntArray kdl_angles(num_joints_), kdl_velocities(num_joints_), kdl_accelerations(num_joints_), kdl_torques(num_joints_);
00182 KDL::Wrenches kdl_wrenches(num_segments_);
00183
00184 for (unsigned int i = 0; i < num_joints_; ++i)
00185 {
00186 kdl_angles(i) = joint_angles[i];
00187 kdl_velocities(i) = joint_velocities[i];
00188 kdl_accelerations(i) = joint_accelerations[i];
00189 }
00190
00191 for (unsigned int i = 0; i < num_segments_; ++i)
00192 {
00193 kdl_wrenches[i](0) = wrenches[i].force.x;
00194 kdl_wrenches[i](1) = wrenches[i].force.y;
00195 kdl_wrenches[i](2) = wrenches[i].force.z;
00196
00197 kdl_wrenches[i](3) = wrenches[i].torque.x;
00198 kdl_wrenches[i](4) = wrenches[i].torque.y;
00199 kdl_wrenches[i](5) = wrenches[i].torque.z;
00200 }
00201
00202 if (chain_id_solver_->CartToJnt(kdl_angles, kdl_velocities, kdl_accelerations, kdl_wrenches, kdl_torques) < 0)
00203 {
00204 logError("moveit.dynamics_solver: Something went wrong computing torques");
00205 return false;
00206 }
00207
00208 for (unsigned int i = 0; i < num_joints_; ++i)
00209 torques[i] = kdl_torques(i);
00210
00211 return true;
00212 }
00213
00214 bool DynamicsSolver::getMaxPayload(const std::vector<double> &joint_angles,
00215 double &payload,
00216 unsigned int &joint_saturated) const
00217 {
00218 if (!joint_model_group_)
00219 {
00220 logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. Check error logs.");
00221 return false;
00222 }
00223 if (joint_angles.size() != num_joints_)
00224 {
00225 logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
00226 return false;
00227 }
00228 std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
00229 std::vector<double> torques(num_joints_, 0.0), zero_torques(num_joints_, 0.0);
00230
00231 std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
00232 if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, zero_torques))
00233 return false;
00234
00235 for (unsigned int i = 0; i < num_joints_; ++i)
00236 {
00237 if (fabs(zero_torques[i]) >= max_torques_[i])
00238 {
00239 payload = 0.0;
00240 joint_saturated = i;
00241 return true;
00242 }
00243 }
00244
00245 state_->setJointGroupPositions(joint_model_group_, joint_angles);
00246 const Eigen::Affine3d &base_frame = state_->getFrameTransform(base_name_);
00247 const Eigen::Affine3d &tip_frame = state_->getFrameTransform(tip_name_);
00248 Eigen::Affine3d transform = tip_frame.inverse() * base_frame;
00249 wrenches.back().force.z = 1.0;
00250 wrenches.back().force = transformVector(transform, wrenches.back().force);
00251 wrenches.back().torque = transformVector(transform, wrenches.back().torque);
00252
00253 logDebug("moveit.dynamics_solver: New wrench (local frame): %f %f %f", wrenches.back().force.x, wrenches.back().force.y, wrenches.back().force.z);
00254
00255 if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, torques))
00256 return false;
00257
00258 double min_payload = std::numeric_limits<double>::max();
00259 for (unsigned int i = 0; i < num_joints_; ++i)
00260 {
00261 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]));
00262 logDebug("moveit.dynamics_solver: Joint: %d, Actual Torque: %f, Max Allowed: %f, Gravity: %f", i, torques[i], max_torques_[i], zero_torques[i]);
00263 logDebug("moveit.dynamics_solver: Joint: %d, Payload Allowed (N): %f", i, payload_joint);
00264 if (payload_joint < min_payload)
00265 {
00266 min_payload = payload_joint;
00267 joint_saturated = i;
00268 }
00269 }
00270 payload = min_payload/gravity_;
00271 logDebug("moveit.dynamics_solver: Max payload (kg): %f", payload);
00272 return true;
00273 }
00274
00275 bool DynamicsSolver::getPayloadTorques(const std::vector<double> &joint_angles,
00276 double payload,
00277 std::vector<double> &joint_torques) const
00278 {
00279 if (!joint_model_group_)
00280 {
00281 logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. Check error logs.");
00282 return false;
00283 }
00284 if (joint_angles.size() != num_joints_)
00285 {
00286 logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
00287 return false;
00288 }
00289 if (joint_torques.size() != num_joints_)
00290 {
00291 logError("moveit.dynamics_solver: Joint torques vector should be size %d", num_joints_);
00292 return false;
00293 }
00294 std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
00295 std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
00296 state_->setJointGroupPositions(joint_model_group_, joint_angles);
00297
00298 const Eigen::Affine3d &base_frame = state_->getFrameTransform(base_name_);
00299 const Eigen::Affine3d &tip_frame = state_->getFrameTransform(tip_name_);
00300 Eigen::Affine3d transform = tip_frame.inverse()* base_frame;
00301 wrenches.back().force.z = payload * gravity_;
00302 wrenches.back().force = transformVector(transform, wrenches.back().force);
00303 wrenches.back().torque = transformVector(transform, wrenches.back().torque);
00304
00305 logDebug("moveit.dynamics_solver: New wrench (local frame): %f %f %f", wrenches.back().force.x, wrenches.back().force.y, wrenches.back().force.z);
00306
00307 if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, joint_torques))
00308 return false;
00309 return true;
00310 }
00311
00312 const std::vector<double>& DynamicsSolver::getMaxTorques() const
00313 {
00314 return max_torques_;
00315 }
00316
00317 }