diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_torque_limit_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_torque_limit_parameterization.h index cfee260535..7c030b3f2f 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_torque_limit_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_torque_limit_parameterization.h @@ -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 @@ -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& joint_torque_limits, - double accel_limit_decrement_factor, - const std::unordered_map& velocity_limits, - const std::unordered_map& 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& external_link_wrenches, const std::vector& joint_torque_limits, + double accel_limit_decrement_factor, const std::unordered_map& velocity_limits, + const std::unordered_map& acceleration_limits, const double max_velocity_scaling_factor, + const double max_acceleration_scaling_factor) const; private: std::optional totg_; diff --git a/moveit_core/trajectory_processing/src/iterative_torque_limit_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_torque_limit_parameterization.cpp index a13bd9cb0a..f536aba338 100644 --- a/moveit_core/trajectory_processing/src/iterative_torque_limit_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_torque_limit_parameterization.cpp @@ -52,8 +52,8 @@ IterativeTorqueLimitParameterization::IterativeTorqueLimitParameterization(const bool IterativeTorqueLimitParameterization::computeTimeStampsWithTorqueLimits( robot_trajectory::RobotTrajectory& trajectory, const geometry_msgs::Vector3& gravity_vector, - const std::vector& joint_torque_limits, double accel_limit_decrement_factor, - const std::unordered_map& velocity_limits, + const std::vector& external_link_wrenches, const std::vector& joint_torque_limits, + double accel_limit_decrement_factor, const std::unordered_map& velocity_limits, const std::unordered_map& acceleration_limits, const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor) const { @@ -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 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 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); @@ -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."); @@ -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; diff --git a/moveit_core/trajectory_processing/test/test_iterative_torque_limit_parameterization.cpp b/moveit_core/trajectory_processing/test/test_iterative_torque_limit_parameterization.cpp index 5c464b8409..6ac4daaeb6 100644 --- a/moveit_core/trajectory_processing/test/test_iterative_torque_limit_parameterization.cpp +++ b/moveit_core/trajectory_processing/test/test_iterative_torque_limit_parameterization.cpp @@ -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 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 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 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";