Skip to content

Commit

Permalink
Merge pull request #22 from AcutronicRobotics/dynamics_solver_last
Browse files Browse the repository at this point in the history 
Port Dynamics solver
  • Loading branch information
@ahcorde
ahcorde committed Mar 5, 2019
2 parents 34849cc + e092774 commit 58049d2
Show file tree
Hide file tree
Showing 4 changed files with 50 additions and 43 deletions.
6 changes: 3 additions & 3 deletions moveit_core/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS
controller_manager/include
distance_field/include
collision_distance_field/include
# dynamics_solver/include
dynamics_solver/include
kinematics_base/include
kinematics_metrics/include
robot_model/include
Expand All @@ -108,7 +108,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS
profiler/include
sensor_manager/include
trajectory_processing/include
# utils/include
utils/include
logging/include
)

Expand Down Expand Up @@ -200,7 +200,7 @@ add_subdirectory(trajectory_processing)
add_subdirectory(distance_field)
add_subdirectory(collision_distance_field)
add_subdirectory(kinematics_metrics)
# add_subdirectory(dynamics_solver)
add_subdirectory(dynamics_solver)
add_subdirectory(utils)

# ament_package()
13 changes: 8 additions & 5 deletions moveit_core/dynamics_solver/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,14 @@ set(MOVEIT_LIB_NAME moveit_dynamics_solver)
add_library(${MOVEIT_LIB_NAME} src/dynamics_solver.cpp)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_state ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
ament_target_dependencies(${MOVEIT_LIB_NAME}
moveit_robot_state
urdf
urdfdom_headers)

install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib)

install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
install(DIRECTORY include/
DESTINATION include)
12 changes: 8 additions & 4 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 @@ -42,16 +42,20 @@
#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 <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 All @@ -68,7 +72,7 @@ class DynamicsSolver
* @return False if initialization failed
*/
DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name,
const geometry_msgs::Vector3& gravity_vector);
const geometry_msgs::msg::Vector3& gravity_vector);

/**
* @brief Get the torques (the order of all input and output is the same
Expand All @@ -86,7 +90,7 @@ class DynamicsSolver
* @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,
const std::vector<double>& joint_accelerations, const std::vector<geometry_msgs::msg::Wrench>& wrenches,
std::vector<double>& torques) const;

/**
Expand Down
62 changes: 31 additions & 31 deletions moveit_core/dynamics_solver/src/dynamics_solver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,13 +49,13 @@ namespace dynamics_solver
{
namespace
{
inline geometry_msgs::Vector3 transformVector(const Eigen::Isometry3d& transform, const geometry_msgs::Vector3& vector)
inline geometry_msgs::msg::Vector3 transformVector(const Eigen::Isometry3d& transform, const geometry_msgs::msg::Vector3& vector)
{
Eigen::Vector3d p;
p = Eigen::Vector3d(vector.x, vector.y, vector.z);
p = transform.rotation() * p;

geometry_msgs::Vector3 result;
geometry_msgs::msg::Vector3 result;
result.x = p.x();
result.y = p.y();
result.z = p.z();
Expand All @@ -65,7 +65,7 @@ inline geometry_msgs::Vector3 transformVector(const Eigen::Isometry3d& transform
}

DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name,
const geometry_msgs::Vector3& gravity_vector)
const geometry_msgs::msg::Vector3& gravity_vector)
{
robot_model_ = robot_model;
joint_model_group_ = robot_model_->getJointModelGroup(group_name);
Expand All @@ -74,15 +74,15 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode

if (!joint_model_group_->isChain())
{
ROS_ERROR_NAMED("dynamics_solver", "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())
{
ROS_ERROR_NAMED("dynamics_solver", "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 +91,29 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode
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());
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();
ROS_DEBUG_NAMED("dynamics_solver", "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))
{
ROS_ERROR_NAMED("dynamics_solver", "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_))
{
ROS_ERROR_NAMED("dynamics_solver", "Could not initialize chain object");
RCLCPP_ERROR(logger, "Could not initialize chain object");
joint_model_group_ = nullptr;
return;
}
Expand All @@ -136,44 +136,44 @@ 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();
ROS_DEBUG_NAMED("dynamics_solver", "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));
}

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
const std::vector<geometry_msgs::msg::Wrench>& wrenches, std::vector<double>& torques) const
{
if (!joint_model_group_)
{
ROS_DEBUG_NAMED("dynamics_solver", "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_)
{
ROS_ERROR_NAMED("dynamics_solver", "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_)
{
ROS_ERROR_NAMED("dynamics_solver", "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_)
{
ROS_ERROR_NAMED("dynamics_solver", "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_)
{
ROS_ERROR_NAMED("dynamics_solver", "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_)
{
ROS_ERROR_NAMED("dynamics_solver", "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 +201,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)
{
ROS_ERROR_NAMED("dynamics_solver", "Something went wrong computing torques");
RCLCPP_ERROR(logger, "Something went wrong computing torques");
return false;
}

Expand All @@ -216,19 +216,19 @@ bool DynamicsSolver::getMaxPayload(const std::vector<double>& joint_angles, doub
{
if (!joint_model_group_)
{
ROS_DEBUG_NAMED("dynamics_solver", "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_)
{
ROS_ERROR_NAMED("dynamics_solver", "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);
std::vector<double> torques(num_joints_, 0.0), zero_torques(num_joints_, 0.0);

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

Expand All @@ -250,7 +250,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);

ROS_DEBUG_NAMED("dynamics_solver", "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 +262,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
ROS_DEBUG_NAMED("dynamics_solver", "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]);
ROS_DEBUG_NAMED("dynamics_solver", "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_;
ROS_DEBUG_NAMED("dynamics_solver", "Max payload (kg): %f", payload);
RCLCPP_DEBUG(logger, "Max payload (kg): %f", payload);
return true;
}

Expand All @@ -281,22 +281,22 @@ bool DynamicsSolver::getPayloadTorques(const std::vector<double>& joint_angles,
{
if (!joint_model_group_)
{
ROS_DEBUG_NAMED("dynamics_solver", "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_)
{
ROS_ERROR_NAMED("dynamics_solver", "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_)
{
ROS_ERROR_NAMED("dynamics_solver", "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);
std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
std::vector<geometry_msgs::msg::Wrench> wrenches(num_segments_);
state_->setJointGroupPositions(joint_model_group_, joint_angles);

const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_);
Expand All @@ -306,7 +306,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);

ROS_DEBUG_NAMED("dynamics_solver", "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 All @@ -317,4 +317,4 @@ const std::vector<double>& DynamicsSolver::getMaxTorques() const
return max_torques_;
}

} // end of namespace dynamics_solver
} // end of namespace dynamics_solver

0 comments on commit 58049d2

Please sign in to comment.