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.
attic: Move RBT DiffIK implementation to attic (RobotLocomotion#10047)
This removes the runtime dependency from //manipulation/planner into //attic. The unit test is still based on RigidBodyTree.
- Loading branch information
1 parent
a5bbf14
commit a231084
Showing
7 changed files
with
190 additions
and
96 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
38 changes: 38 additions & 0 deletions
38
attic/manipulation/planner/rbt_differential_inverse_kinematics.cc
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,38 @@ | ||
| #include "drake/manipulation/planner/rbt_differential_inverse_kinematics.h" | ||
|
|
||
| namespace drake { | ||
| namespace manipulation { | ||
| namespace planner { | ||
| namespace rbt { | ||
|
|
||
| DifferentialInverseKinematicsResult DoDifferentialInverseKinematics( | ||
| const RigidBodyTree<double>& robot, const KinematicsCache<double>& cache, | ||
| const Isometry3<double>& X_WE_desired, | ||
| const RigidBodyFrame<double>& frame_E, | ||
| const DifferentialInverseKinematicsParameters& parameters) { | ||
| const Isometry3<double> X_WE = | ||
| robot.CalcFramePoseInWorldFrame(cache, frame_E); | ||
| const Vector6<double> V_WE_desired = | ||
| ComputePoseDiffInCommonFrame(X_WE, X_WE_desired) / | ||
| parameters.get_timestep(); | ||
| // Call the below function. | ||
| return drake::manipulation::planner::rbt::DoDifferentialInverseKinematics( | ||
| robot, cache, V_WE_desired, frame_E, parameters); | ||
| } | ||
|
|
||
| DifferentialInverseKinematicsResult DoDifferentialInverseKinematics( | ||
| const RigidBodyTree<double>& robot, const KinematicsCache<double>& cache, | ||
| const Vector6<double>& V_WE_desired, const RigidBodyFrame<double>& frame_E, | ||
| const DifferentialInverseKinematicsParameters& parameters) { | ||
| Isometry3<double> X_WE = robot.CalcFramePoseInWorldFrame(cache, frame_E); | ||
| MatrixX<double> J_WE = | ||
| robot.CalcFrameSpatialVelocityJacobianInWorldFrame(cache, frame_E); | ||
| // Call the (non-attic) helper function. | ||
| return drake::manipulation::planner::detail::DoDifferentialInverseKinematics( | ||
| cache.getQ(), cache.getV(), X_WE, J_WE, V_WE_desired, parameters); | ||
| } | ||
|
|
||
| } // namespace rbt | ||
| } // namespace planner | ||
| } // namespace manipulation | ||
| } // namespace drake |
60 changes: 60 additions & 0 deletions
60
attic/manipulation/planner/rbt_differential_inverse_kinematics.h
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,60 @@ | ||
| #pragma once | ||
|
|
||
| #include "drake/manipulation/planner/differential_inverse_kinematics.h" | ||
| #include "drake/multibody/rigid_body_tree.h" | ||
|
|
||
| namespace drake { | ||
| namespace manipulation { | ||
| namespace planner { | ||
| namespace rbt { | ||
|
|
||
| /** | ||
| * A wrapper over | ||
| * DoDifferentialInverseKinematics(q_current, v_current, V, J, params) | ||
| * that tracks frame E's spatial velocity. | ||
| * q_current and v_current are taken from @p cache. V is computed by first | ||
| * transforming @p V_WE to V_WE_E, then taking the element-wise product between | ||
| * V_WE_E and the gains (specified in frame E) in @p parameters, and only | ||
| * selecting the non zero elements. J is computed similarly. | ||
| * @param robot Kinematic tree. | ||
| * @param cache Kinematic cache build from the current generalized position and | ||
| * velocity. | ||
| * @param V_WE_desired Desired world frame spatial velocity of @p frame_E. | ||
| * @param frame_E End effector frame. | ||
| * @param parameters Collection of various problem specific constraints and | ||
| * constants. | ||
| * @return If the solver successfully finds a solution, joint_velocities will | ||
| * be set to v, otherwise it will be nullopt. | ||
| */ | ||
| DifferentialInverseKinematicsResult DoDifferentialInverseKinematics( | ||
| const RigidBodyTree<double>& robot, const KinematicsCache<double>& cache, | ||
| const Vector6<double>& V_WE_desired, const RigidBodyFrame<double>& frame_E, | ||
| const DifferentialInverseKinematicsParameters& parameters); | ||
|
|
||
| /** | ||
| * A wrapper over | ||
| * DoDifferentialInverseKinematics(robot, cache, V_WE_desired, frame_E, params) | ||
| * that tracks frame E's pose in the world frame. | ||
| * q_current and v_current are taken from @p cache. V_WE is computed by | ||
| * ComputePoseDiffInCommonFrame(X_WE, X_WE_desired) / dt, where X_WE is computed | ||
| * from @p cache, and dt is taken from @p parameters. | ||
| * @param robot Robot model. | ||
| * @param cache KinematiCache built from the current generalized position and | ||
| * velocity. | ||
| * @param X_WE_desired Desired pose of @p frame_E in the world frame. | ||
| * @param frame_E End effector frame. | ||
| * @param parameters Collection of various problem specific constraints and | ||
| * constants. | ||
| * @return If the solver successfully finds a solution, joint_velocities will | ||
| * be set to v, otherwise it will be nullopt. | ||
| */ | ||
| DifferentialInverseKinematicsResult DoDifferentialInverseKinematics( | ||
| const RigidBodyTree<double>& robot, const KinematicsCache<double>& cache, | ||
| const Isometry3<double>& X_WE_desired, | ||
| const RigidBodyFrame<double>& frame_E, | ||
| const DifferentialInverseKinematicsParameters& parameters); | ||
|
|
||
| } // namespace rbt | ||
| } // namespace planner | ||
| } // namespace manipulation | ||
| } // namespace drake |
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
Oops, something went wrong.