forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Move some helper functions from iiwa to manipulation/util (RobotLocom…
…otion#13015) * Move some helper functions from iiwa to manipulation/util These are items I've used in personal branches exploring planning/sim with different robots that don't currently have drake examples (or at least hadn't used this functionality yet).
- Loading branch information
Showing
8 changed files
with
312 additions
and
67 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,132 @@ | ||
| #include "drake/manipulation/util/robot_plan_utils.h" | ||
|
|
||
| #include <map> | ||
|
|
||
| #include "drake/common/default_scalars.h" | ||
| #include "drake/common/drake_assert.h" | ||
|
|
||
| namespace drake { | ||
| namespace manipulation { | ||
| namespace util { | ||
|
|
||
| /// @return A vector of joint names corresponding to the positions in @plant | ||
| /// in the order of the joint indices. | ||
| template <typename T> | ||
| std::vector<std::string> GetJointNames( | ||
| const multibody::MultibodyPlant<T>& plant) { | ||
|
|
||
| std::map<int, std::string> position_names; | ||
| const int num_positions = plant.num_positions(); | ||
| for (int i = 0; i < plant.num_joints(); ++i) { | ||
| const multibody::Joint<T>& joint = | ||
| plant.get_joint(multibody::JointIndex(i)); | ||
| if (joint.num_positions() == 0) { | ||
| continue; | ||
| } | ||
| DRAKE_DEMAND(joint.num_positions() == 1); | ||
| DRAKE_DEMAND(joint.position_start() < num_positions); | ||
|
|
||
| position_names[joint.position_start()] = joint.name(); | ||
| } | ||
|
|
||
| DRAKE_DEMAND(static_cast<int>(position_names.size()) == num_positions); | ||
| std::vector<std::string> joint_names; | ||
| for (int i = 0; i < num_positions; ++i) { | ||
| joint_names.push_back(position_names[i]); | ||
| } | ||
| return joint_names; | ||
| } | ||
|
|
||
| void ApplyJointVelocityLimits( | ||
| const std::vector<Eigen::VectorXd>& keyframes, | ||
| const Eigen::VectorXd& limits, | ||
| std::vector<double>* times) { | ||
| DRAKE_DEMAND(keyframes.size() == times->size()); | ||
| DRAKE_DEMAND(times->front() ==0); | ||
| const int num_time_steps = keyframes.size(); | ||
|
|
||
| // Calculate a matrix of velocities between each timestep. We'll | ||
| // use this later to determine by how much the plan exceeds the | ||
| // joint velocity limits. | ||
| Eigen::MatrixXd velocities(limits.size(), num_time_steps - 1); | ||
| for (int i = 0; i < velocities.rows(); i++) { | ||
| for (int j = 0; j < velocities.cols(); j++) { | ||
| DRAKE_ASSERT((*times)[j + 1] > (*times)[j]); | ||
| velocities(i, j) = | ||
| std::abs((keyframes[j + 1](i) - keyframes[j](i)) / | ||
| ((*times)[j + 1] - (*times)[j])); | ||
| } | ||
| } | ||
|
|
||
| Eigen::VectorXd velocity_ratios(velocities.rows()); | ||
|
|
||
| for (int i = 0; i < velocities.rows(); i++) { | ||
| const double max_plan_velocity = velocities.row(i).maxCoeff(); | ||
| velocity_ratios(i) = max_plan_velocity / limits(i); | ||
| } | ||
|
|
||
| const double max_velocity_ratio = velocity_ratios.maxCoeff(); | ||
| if (max_velocity_ratio > 1) { | ||
| // The code below slows the entire plan such that the fastest step | ||
| // meets the limits. If that step is much faster than the others, | ||
| // the whole plan becomes very slow. | ||
| drake::log()->debug("Slowing plan by {}", max_velocity_ratio); | ||
| for (int j = 0; j < num_time_steps; j++) { | ||
| (*times)[j] *= max_velocity_ratio; | ||
| } | ||
| } | ||
| } | ||
|
|
||
| robotlocomotion::robot_plan_t EncodeKeyFrames( | ||
| const std::vector<std::string>& joint_names, | ||
| const std::vector<double>& times, | ||
| const std::vector<int>& info, | ||
| const std::vector<Eigen::VectorXd>& keyframes) { | ||
| DRAKE_DEMAND(info.size() == times.size()); | ||
| DRAKE_DEMAND(keyframes.size() == times.size()); | ||
|
|
||
| const int num_time_steps = keyframes.size(); | ||
|
|
||
| robotlocomotion::robot_plan_t plan{}; | ||
| plan.utime = 0; // I (sam.creasey) don't think this is used? | ||
| plan.robot_name = ""; // Arbitrary, probably ignored | ||
| plan.num_states = num_time_steps; | ||
| const bot_core::robot_state_t default_robot_state{}; | ||
| plan.plan.resize(num_time_steps, default_robot_state); | ||
| plan.plan_info.resize(num_time_steps, 0); | ||
| /// Encode the q_sol returned for each timestep into the vector of | ||
| /// robot states. | ||
| for (int i = 0; i < num_time_steps; i++) { | ||
| DRAKE_DEMAND(keyframes[i].size() == static_cast<int>(joint_names.size())); | ||
| bot_core::robot_state_t& step = plan.plan[i]; | ||
| step.utime = times[i] * 1e6; | ||
| step.num_joints = keyframes[i].size(); | ||
| for (int j = 0; j < step.num_joints; j++) { | ||
| step.joint_name.push_back(joint_names[j]); | ||
| step.joint_position.push_back(keyframes[i](j)); | ||
| step.joint_velocity.push_back(0); | ||
| step.joint_effort.push_back(0); | ||
| } | ||
| plan.plan_info[i] = info[i]; | ||
| } | ||
| plan.num_grasp_transitions = 0; | ||
| plan.left_arm_control_type = plan.POSITION; | ||
| plan.right_arm_control_type = plan.NONE; | ||
| plan.left_leg_control_type = plan.NONE; | ||
| plan.right_leg_control_type = plan.NONE; | ||
| plan.num_bytes = 0; | ||
|
|
||
| return plan; | ||
| } | ||
|
|
||
|
|
||
| } // namespace util | ||
| } // namespace manipulation | ||
| } // namespace drake | ||
|
|
||
| template std::vector<std::string> drake::manipulation::util::GetJointNames( | ||
| const drake::multibody::MultibodyPlant<double>&); | ||
| template std::vector<std::string> drake::manipulation::util::GetJointNames( | ||
| const drake::multibody::MultibodyPlant<::drake::AutoDiffXd>&); | ||
| template std::vector<std::string> drake::manipulation::util::GetJointNames( | ||
| const drake::multibody::MultibodyPlant<::drake::symbolic::Expression>&); |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,50 @@ | ||
| #pragma once | ||
|
|
||
| /// @file Functions to help with the creation of robot_plan_t messages. | ||
|
|
||
| #include <string> | ||
| #include <vector> | ||
|
|
||
| #include "robotlocomotion/robot_plan_t.hpp" | ||
|
|
||
| #include "drake/multibody/plant/multibody_plant.h" | ||
|
|
||
| namespace drake { | ||
| namespace manipulation { | ||
| namespace util { | ||
|
|
||
| /// @return A vector of joint names corresponding to the positions in @p plant | ||
| /// in the order of the joint indices. If joints with duplicate names exist | ||
| /// in different model instance in the plant, the names will be duplicated in | ||
| /// the output. | ||
| template <typename T> | ||
| std::vector<std::string> GetJointNames( | ||
| const multibody::MultibodyPlant<T>& plant); | ||
|
|
||
| /// Scales a plan so that no step exceeds the robot's maximum joint | ||
| /// velocities. The size of @p keyframes must match the size of @p times. | ||
| /// Times must be in strictly increasing order and start with zero. Per-joint | ||
| /// velocity limits are specified by @p limits, which much be the same size ad | ||
| /// the number of joints in each element of @p keyframes. Assumes that | ||
| /// velocity limits are equal regardless of direction. If any step does | ||
| /// exceed the maximum velocities in @p limits, @p times will be modified to | ||
| /// reduce the velocity. | ||
| void ApplyJointVelocityLimits( | ||
| const std::vector<Eigen::VectorXd>& keyframes, | ||
| const Eigen::VectorXd& limits, | ||
| std::vector<double>* times); | ||
|
|
||
| /// Makes a robotlocomotion::robot_plan_t message. The entries in @p | ||
| /// joint_names should be unique, though the behavior if names are duplicated | ||
| /// depends on how the returned plan is evaluated. The size of each vector in | ||
| /// @p keyframes must match the size of @p joint_names. The size of @p | ||
| /// keyframes must match the size of @p times. Times must be in strictly | ||
| /// increasing order. | ||
| robotlocomotion::robot_plan_t EncodeKeyFrames( | ||
| const std::vector<std::string>& joint_names, | ||
| const std::vector<double>& times, const std::vector<int>& info, | ||
| const std::vector<Eigen::VectorXd>& keyframes); | ||
|
|
||
| } // namespace util | ||
| } // namespace manipulation | ||
| } // namespace drake |
Oops, something went wrong.