43 const std::string
LOGNAME =
"trajectory_processing.iterative_torque_limit_parameterization";
47 const double resample_dt,
48 const double min_angle_change)
49 : totg_(path_tolerance, resample_dt, min_angle_change)
55 const std::vector<geometry_msgs::Wrench>& external_link_wrenches,
const std::vector<double>& joint_torque_limits,
56 double accel_limit_decrement_factor,
const std::unordered_map<std::string, double>& velocity_limits,
57 const std::unordered_map<std::string, double>& acceleration_limits,
const double max_velocity_scaling_factor,
58 const double max_acceleration_scaling_factor)
const
64 if (trajectory.
empty())
75 std::unordered_map<std::string, double> mutable_accel_limits = acceleration_limits;
79 if (joint_torque_limits.size() != dof)
82 <<
") does not match the DOF of the RobotModel ("
87 if (accel_limit_decrement_factor < 0.01 || accel_limit_decrement_factor > 0.2)
96 moveit_msgs::RobotTrajectory original_traj;
100 bool iteration_needed =
true;
101 size_t num_iterations = 0;
102 const size_t max_iterations = 10;
104 while (iteration_needed && num_iterations < max_iterations)
107 iteration_needed =
false;
110 max_acceleration_scaling_factor);
112 std::vector<double> joint_positions(dof);
113 std::vector<double> joint_velocities(dof);
114 std::vector<double> joint_accelerations(dof);
115 std::vector<double> joint_torques(dof);
117 const std::vector<const moveit::core::JointModel*>& joint_models = group->
getActiveJointModels();
120 for (
size_t waypoint_idx = 0; waypoint_idx < trajectory.
getWayPointCount(); ++waypoint_idx)
122 moveit::core::RobotStatePtr& waypoint = trajectory.
getWayPointPtr(waypoint_idx);
123 waypoint->copyJointGroupPositions(group->
getName(), joint_positions);
124 waypoint->copyJointGroupVelocities(group->
getName(), joint_velocities);
125 waypoint->copyJointGroupAccelerations(group->
getName(), joint_accelerations);
127 if (!
dynamics_solver.getTorques(joint_positions, joint_velocities, joint_accelerations, external_link_wrenches,
135 for (
size_t joint_idx = 0; joint_idx < joint_torque_limits.size(); ++joint_idx)
137 if (std::fabs(joint_torques.at(joint_idx)) > joint_torque_limits.at(joint_idx))
147 waypoint->copyJointGroupAccelerations(group->
getName(), joint_accelerations);
150 double previous_torque = joint_torques.at(joint_idx);
151 joint_accelerations.at(joint_idx) *= (1 + accel_limit_decrement_factor);
152 if (!
dynamics_solver.getTorques(joint_positions, joint_velocities, joint_accelerations,
153 external_link_wrenches, joint_torques))
158 if (std::fabs(joint_torques.at(joint_idx)) < std::fabs(previous_torque))
160 mutable_accel_limits.at(joint_models.at(joint_idx)->getName()) *= (1 + accel_limit_decrement_factor);
164 mutable_accel_limits.at(joint_models.at(joint_idx)->getName()) *= (1 - accel_limit_decrement_factor);
167 mutable_accel_limits.at(joint_models.at(joint_idx)->getName()) *= (1 - accel_limit_decrement_factor);
168 iteration_needed =
true;
171 if (iteration_needed)
178 if (iteration_needed)