Skip to content

Commit

Permalink
Make external_link_wrenches into an argument
Browse files Browse the repository at this point in the history
  • Loading branch information
@AndyZe
AndyZe committed May 5, 2023
1 parent 488b175 commit 65240b9
Show file tree
Hide file tree
Showing 3 changed files with 30 additions and 30 deletions.
15 changes: 7 additions & 8 deletions ...processing/include/moveit/trajectory_processing/iterative_torque_limit_parameterization.h
View file
Expand Up @@ -56,6 +56,7 @@ class IterativeTorqueLimitParameterization
* \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been
* time-parameterized; this function will re-time-parameterize it.
* \param gravity_vector For example, (0, 0, -9.81). Units are m/s^2
* \param external_link_wrenches Externally-applied wrenches on each link. TODO(andyz): what frame is this in?
* \param joint_torque_limits Torque limits for each joint in N*m. Should all be >0.
* \param accel_limit_decrement_factor Typically in the range [0.01-0.1].
* This affects how fast acceleration limits are decreased while searching for a solution. Time-optimality
Expand All @@ -64,14 +65,12 @@ class IterativeTorqueLimitParameterization
* \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
* \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
*/
bool computeTimeStampsWithTorqueLimits(robot_trajectory::RobotTrajectory& trajectory,
const geometry_msgs::Vector3& gravity_vector,
const std::vector<double>& joint_torque_limits,
double accel_limit_decrement_factor,
const std::unordered_map<std::string, double>& velocity_limits,
const std::unordered_map<std::string, double>& acceleration_limits,
const double max_velocity_scaling_factor,
const double max_acceleration_scaling_factor) const;
bool computeTimeStampsWithTorqueLimits(
robot_trajectory::RobotTrajectory& trajectory, const geometry_msgs::Vector3& gravity_vector,
const std::vector<geometry_msgs::Wrench>& external_link_wrenches, const std::vector<double>& joint_torque_limits,
double accel_limit_decrement_factor, const std::unordered_map<std::string, double>& velocity_limits,
const std::unordered_map<std::string, double>& acceleration_limits, const double max_velocity_scaling_factor,
const double max_acceleration_scaling_factor) const;

private:
std::optional<TimeOptimalTrajectoryGeneration> totg_;
Expand Down
24 changes: 5 additions & 19 deletions moveit_core/trajectory_processing/src/iterative_torque_limit_parameterization.cpp
View file
Expand Up @@ -52,8 +52,8 @@ IterativeTorqueLimitParameterization::IterativeTorqueLimitParameterization(const

bool IterativeTorqueLimitParameterization::computeTimeStampsWithTorqueLimits(
robot_trajectory::RobotTrajectory& trajectory, const geometry_msgs::Vector3& gravity_vector,
const std::vector<double>& joint_torque_limits, double accel_limit_decrement_factor,
const std::unordered_map<std::string, double>& velocity_limits,
const std::vector<geometry_msgs::Wrench>& external_link_wrenches, const std::vector<double>& joint_torque_limits,
double accel_limit_decrement_factor, const std::unordered_map<std::string, double>& velocity_limits,
const std::unordered_map<std::string, double>& acceleration_limits, const double max_velocity_scaling_factor,
const double max_acceleration_scaling_factor) const
{
Expand Down Expand Up @@ -92,20 +92,6 @@ bool IterativeTorqueLimitParameterization::computeTimeStampsWithTorqueLimits(

dynamics_solver::DynamicsSolver dynamics_solver(trajectory.getRobotModel(), group->getName(), gravity_vector);

// Assume no external forces on the robot. This could easily be an argument later.
const std::vector<geometry_msgs::Wrench> link_wrenches = [&group] {
geometry_msgs::Wrench zero_wrench;
zero_wrench.force.x = 0;
zero_wrench.force.y = 0;
zero_wrench.force.z = 0;
zero_wrench.torque.x = 0;
zero_wrench.torque.y = 0;
zero_wrench.torque.z = 0;
// KDL (the dynamics solver) requires one wrench per link
std::vector<geometry_msgs::Wrench> vector_of_zero_wrenches(group->getLinkModels().size(), zero_wrench);
return vector_of_zero_wrenches;
}();

// Copy the waypoints so we can modify them while iterating
moveit_msgs::RobotTrajectory original_traj;
trajectory.getRobotTrajectoryMsg(original_traj);
Expand Down Expand Up @@ -144,7 +130,7 @@ bool IterativeTorqueLimitParameterization::computeTimeStampsWithTorqueLimits(
waypoint->copyJointGroupVelocities(group->getName(), joint_velocities);
waypoint->copyJointGroupAccelerations(group->getName(), joint_accelerations);

if (!dynamics_solver.getTorques(joint_positions, joint_velocities, joint_accelerations, link_wrenches,
if (!dynamics_solver.getTorques(joint_positions, joint_velocities, joint_accelerations, external_link_wrenches,
joint_torques))
{
ROS_ERROR_STREAM_NAMED(LOGNAME, "Dynamics computation failed.");
Expand All @@ -169,8 +155,8 @@ bool IterativeTorqueLimitParameterization::computeTimeStampsWithTorqueLimits(
// Check if decreasing acceleration of this joint actually decreases joint torque. Else, increase acceleration.
double previous_torque = joint_torques.at(joint_idx);
joint_accelerations.at(joint_idx) *= (1 + accel_limit_decrement_factor);
if (!dynamics_solver.getTorques(joint_positions, joint_velocities, joint_accelerations, link_wrenches,
joint_torques))
if (!dynamics_solver.getTorques(joint_positions, joint_velocities, joint_accelerations,
external_link_wrenches, joint_torques))
{
ROS_ERROR_STREAM_NAMED(LOGNAME, "Dynamics computation failed.");
return false;
Expand Down
21 changes: 18 additions & 3 deletions moveit_core/trajectory_processing/test/test_iterative_torque_limit_parameterization.cpp
View file
Expand Up @@ -124,17 +124,32 @@ TEST(time_optimal_trajectory_generation, test_totg_with_torque_limits)

trajectory_processing::IterativeTorqueLimitParameterization totg(0.1 /* path tolerance */, 0.01 /* resample dt */,
0.001 /* min angle change */);

// Assume no external forces on the robot.
const std::vector<geometry_msgs::Wrench> external_link_wrenches = [&group] {
geometry_msgs::Wrench zero_wrench;
zero_wrench.force.x = 0;
zero_wrench.force.y = 0;
zero_wrench.force.z = 0;
zero_wrench.torque.x = 0;
zero_wrench.torque.y = 0;
zero_wrench.torque.z = 0;
// KDL (the dynamics solver) requires one wrench per link
std::vector<geometry_msgs::Wrench> vector_of_zero_wrenches(group->getLinkModels().size(), zero_wrench);
return vector_of_zero_wrenches;
}();

bool totg_success =
totg.computeTimeStampsWithTorqueLimits(trajectory, gravity_vector, joint_torque_limits,
totg.computeTimeStampsWithTorqueLimits(trajectory, gravity_vector, external_link_wrenches, joint_torque_limits,
accel_limit_decrement_factor, velocity_limits, acceleration_limits,
1.0 /* accel scaling */, 1.0 /* vel scaling */);
1.0 /* vel scaling */, 1.0 /* accel scaling */);
ASSERT_TRUE(totg_success) << "Failed to compute timestamps";
double first_duration = trajectory.getDuration();

// Now decrease joint torque limits and re-time-parameterize. The trajectory duration should be longer.
const std::vector<double> lower_torque_limits{ 1 }; // in N*m
totg_success =
totg.computeTimeStampsWithTorqueLimits(trajectory, gravity_vector, lower_torque_limits,
totg.computeTimeStampsWithTorqueLimits(trajectory, gravity_vector, external_link_wrenches, lower_torque_limits,
accel_limit_decrement_factor, velocity_limits, acceleration_limits,
1.0 /* accel scaling */, 1.0 /* vel scaling */);
ASSERT_TRUE(totg_success) << "Failed to compute timestamps";
Expand Down

0 comments on commit 65240b9

Please sign in to comment.