Skip to content

Commit

Permalink
Adapt dynamics_solver to logging moveit#21
Browse files Browse the repository at this point in the history
  • Loading branch information
@vmayoral
vmayoral committed Mar 31, 2019
1 parent d051eba commit 1ba37e3
Show file tree
Hide file tree
Showing 2 changed files with 28 additions and 29 deletions.
5 changes: 0 additions & 5 deletions moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,18 +44,13 @@
#include <moveit/robot_state/robot_state.h>
#include <geometry_msgs/msg/vector3.hpp>
#include <geometry_msgs/msg/wrench.hpp>

#include <memory>

#include "rclcpp/rclcpp.hpp"


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

rclcpp::Logger logger = rclcpp::get_logger("dynamics_solver");
/**
* This solver currently computes the required torques given a
* joint configuration, velocities, accelerations and external wrenches
Expand Down
52 changes: 28 additions & 24 deletions moveit_core/dynamics_solver/src/dynamics_solver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,9 +44,13 @@
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/tree.hpp>
#include "rclcpp/rclcpp.hpp"

namespace dynamics_solver
{

rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("dynamics_solver");

namespace
{
inline geometry_msgs::msg::Vector3 transformVector(const Eigen::Isometry3d& transform, const geometry_msgs::msg::Vector3& vector)
Expand Down Expand Up @@ -74,15 +78,15 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode

if (!joint_model_group_->isChain())
{
RCLCPP_ERROR(logger, "Group '%s' is not a chain. Will not initialize dynamics solver",
RCLCPP_ERROR(LOGGER, "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())
{
RCLCPP_ERROR(logger, "Group '%s' has a mimic joint. Will not initialize dynamics solver",
RCLCPP_ERROR(LOGGER, "Group '%s' has a mimic joint. Will not initialize dynamics solver",
group_name.c_str());
joint_model_group_ = nullptr;
return;
Expand All @@ -91,29 +95,29 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode
const robot_model::JointModel* joint = joint_model_group_->getJointRoots()[0];
if (!joint->getParentLinkModel())
{
RCLCPP_ERROR(logger, "Group '%s' does not have a parent link", group_name.c_str());
RCLCPP_ERROR(LOGGER, "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();
RCLCPP_DEBUG(logger, "Base name: '%s', Tip name: '%s'", base_name_.c_str(), tip_name_.c_str());
RCLCPP_DEBUG(LOGGER, "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))
{
RCLCPP_ERROR(logger, "Could not initialize tree object");
RCLCPP_ERROR(LOGGER, "Could not initialize tree object");
joint_model_group_ = nullptr;
return;
}
if (!tree.getChain(base_name_, tip_name_, kdl_chain_))
{
RCLCPP_ERROR(logger, "Could not initialize chain object");
RCLCPP_ERROR(LOGGER, "Could not initialize chain object");
joint_model_group_ = nullptr;
return;
}
Expand All @@ -136,7 +140,7 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode
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();
RCLCPP_DEBUG(logger, "Gravity norm set to %f", gravity_);
RCLCPP_DEBUG(LOGGER, "Gravity norm set to %f", gravity_);

chain_id_solver_.reset(new KDL::ChainIdSolver_RNE(kdl_chain_, gravity));
}
Expand All @@ -147,33 +151,33 @@ bool DynamicsSolver::getTorques(const std::vector<double>& joint_angles, const s
{
if (!joint_model_group_)
{
RCLCPP_DEBUG(logger, "Did not construct DynamicsSolver object properly. "
RCLCPP_DEBUG(LOGGER, "Did not construct DynamicsSolver object properly. "
"Check error logs.");
return false;
}
if (joint_angles.size() != num_joints_)
{
RCLCPP_ERROR(logger, "Joint angles vector should be size %d", num_joints_);
RCLCPP_ERROR(LOGGER, "Joint angles vector should be size %d", num_joints_);
return false;
}
if (joint_velocities.size() != num_joints_)
{
RCLCPP_ERROR(logger, "Joint velocities vector should be size %d", num_joints_);
RCLCPP_ERROR(LOGGER, "Joint velocities vector should be size %d", num_joints_);
return false;
}
if (joint_accelerations.size() != num_joints_)
{
RCLCPP_ERROR(logger, "Joint accelerations vector should be size %d", num_joints_);
RCLCPP_ERROR(LOGGER, "Joint accelerations vector should be size %d", num_joints_);
return false;
}
if (wrenches.size() != num_segments_)
{
RCLCPP_ERROR(logger, "Wrenches vector should be size %d", num_segments_);
RCLCPP_ERROR(LOGGER, "Wrenches vector should be size %d", num_segments_);
return false;
}
if (torques.size() != num_joints_)
{
RCLCPP_ERROR(logger, "Torques vector should be size %d", num_joints_);
RCLCPP_ERROR(LOGGER, "Torques vector should be size %d", num_joints_);
return false;
}

Expand Down Expand Up @@ -201,7 +205,7 @@ bool DynamicsSolver::getTorques(const std::vector<double>& joint_angles, const s

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

Expand All @@ -216,13 +220,13 @@ bool DynamicsSolver::getMaxPayload(const std::vector<double>& joint_angles, doub
{
if (!joint_model_group_)
{
RCLCPP_DEBUG(logger, "Did not construct DynamicsSolver object properly. "
RCLCPP_DEBUG(LOGGER, "Did not construct DynamicsSolver object properly. "
"Check error logs.");
return false;
}
if (joint_angles.size() != num_joints_)
{
RCLCPP_ERROR(logger, "Joint angles vector should be size %d", num_joints_);
RCLCPP_ERROR(LOGGER, "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);
Expand Down Expand Up @@ -250,7 +254,7 @@ bool DynamicsSolver::getMaxPayload(const std::vector<double>& joint_angles, doub
wrenches.back().force = transformVector(transform, wrenches.back().force);
wrenches.back().torque = transformVector(transform, wrenches.back().torque);

RCLCPP_DEBUG(logger, "New wrench (local frame): %f %f %f", wrenches.back().force.x,
RCLCPP_DEBUG(LOGGER, "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))
Expand All @@ -262,17 +266,17 @@ bool DynamicsSolver::getMaxPayload(const std::vector<double>& joint_angles, doub
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
RCLCPP_DEBUG(logger, "Joint: %d, Actual Torque: %f, Max Allowed: %f, Gravity: %f", i, torques[i],
RCLCPP_DEBUG(LOGGER, "Joint: %d, Actual Torque: %f, Max Allowed: %f, Gravity: %f", i, torques[i],
max_torques_[i], zero_torques[i]);
RCLCPP_DEBUG(logger, "Joint: %d, Payload Allowed (N): %f", i, payload_joint);
RCLCPP_DEBUG(LOGGER, "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_;
RCLCPP_DEBUG(logger, "Max payload (kg): %f", payload);
RCLCPP_DEBUG(LOGGER, "Max payload (kg): %f", payload);
return true;
}

Expand All @@ -281,18 +285,18 @@ bool DynamicsSolver::getPayloadTorques(const std::vector<double>& joint_angles,
{
if (!joint_model_group_)
{
RCLCPP_DEBUG(logger, "Did not construct DynamicsSolver object properly. "
RCLCPP_DEBUG(LOGGER, "Did not construct DynamicsSolver object properly. "
"Check error logs.");
return false;
}
if (joint_angles.size() != num_joints_)
{
RCLCPP_ERROR(logger, "Joint angles vector should be size %d", num_joints_);
RCLCPP_ERROR(LOGGER, "Joint angles vector should be size %d", num_joints_);
return false;
}
if (joint_torques.size() != num_joints_)
{
RCLCPP_ERROR(logger, "Joint torques vector should be size %d", num_joints_);
RCLCPP_ERROR(LOGGER, "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);
Expand All @@ -306,7 +310,7 @@ bool DynamicsSolver::getPayloadTorques(const std::vector<double>& joint_angles,
wrenches.back().force = transformVector(transform, wrenches.back().force);
wrenches.back().torque = transformVector(transform, wrenches.back().torque);

RCLCPP_DEBUG(logger, "New wrench (local frame): %f %f %f", wrenches.back().force.x,
RCLCPP_DEBUG(LOGGER, "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);
Expand Down

0 comments on commit 1ba37e3

Please sign in to comment.