Drake
Drake C++ Documentation
differential_inverse_kinematics.h File Reference
#include <limits>
#include <memory>
#include <optional>
#include <ostream>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
#include "drake/common/copyable_unique_ptr.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/eigen_types.h"
#include "drake/common/fmt_ostream.h"
#include "drake/math/rigid_transform.h"
#include "drake/multibody/math/spatial_algebra.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/solvers/mathematical_program.h"
Include dependency graph for differential_inverse_kinematics.h:
This graph shows which files directly or indirectly include this file:

Classes

struct  DifferentialInverseKinematicsResult
 
class  DifferentialInverseKinematicsParameters
 Contains parameters for differential inverse kinematics. More...
 
struct  formatter< drake::multibody::DifferentialInverseKinematicsStatus >
 

Namespaces

 drake
 
 drake::multibody
 
 fmt
 

Enumerations

enum  DifferentialInverseKinematicsStatus { kSolutionFound, kNoSolutionFound, kStuck }
 

Functions

std::ostream & operator<< (std::ostream &os, const DifferentialInverseKinematicsStatus value)
 
Vector6< doubleComputePoseDiffInCommonFrame (const math::RigidTransform< double > &X_C0, const math::RigidTransform< double > &X_C1)
 Computes the pose "difference" between X_C0 and X_C1 such that the linear part equals p_C1 - p_C0, and the angular part equals R_C1 * R_C0.inv(), where p and R stand for the position and rotation parts, and C is the common frame. More...
 
DifferentialInverseKinematicsResult DoDifferentialInverseKinematics (const Eigen::Ref< const VectorX< double >> &q_current, const Eigen::Ref< const VectorX< double >> &v_current, const Eigen::Ref< const VectorX< double >> &V, const Eigen::Ref< const MatrixX< double >> &J, const DifferentialInverseKinematicsParameters &parameters, const std::optional< Eigen::Ref< const Eigen::SparseMatrix< double >>> &N=std::nullopt, const std::optional< Eigen::Ref< const Eigen::SparseMatrix< double >>> &Nplus=std::nullopt)
 Computes a generalized velocity v_next, via the following MathematicalProgram: More...
 
DifferentialInverseKinematicsResult DoDifferentialInverseKinematics (const MultibodyPlant< double > &robot, const systems::Context< double > &context, const Vector6< double > &V_WE_desired, const Frame< double > &frame_E, const DifferentialInverseKinematicsParameters &parameters)
 A wrapper over DoDifferentialInverseKinematics(q_current, v_current, V, J, params) that tracks frame E's spatial velocity. More...
 
DifferentialInverseKinematicsResult DoDifferentialInverseKinematics (const MultibodyPlant< double > &robot, const systems::Context< double > &context, const Vector6< double > &V_AE_desired, const Frame< double > &frame_A, const Frame< double > &frame_E, const DifferentialInverseKinematicsParameters &parameters)
 A wrapper over DoDifferentialInverseKinematics(q_current, v_current, V, J, params) that tracks frame E's spatial velocity in frame A. More...
 
DifferentialInverseKinematicsResult DoDifferentialInverseKinematics (const MultibodyPlant< double > &robot, const systems::Context< double > &context, const math::RigidTransform< double > &X_WE_desired, const Frame< double > &frame_E, const DifferentialInverseKinematicsParameters &parameters)
 A wrapper over DoDifferentialInverseKinematics(robot, context, V_WE_desired, frame_E, params) that tracks frame E's pose in the world frame. More...
 
DifferentialInverseKinematicsResult DoDifferentialInverseKinematics (const MultibodyPlant< double > &robot, const systems::Context< double > &context, const math::RigidTransform< double > &X_AE_desired, const Frame< double > &frame_A, const Frame< double > &frame_E, const DifferentialInverseKinematicsParameters &parameters)
 A wrapper over DoDifferentialInverseKinematics(robot, context, V_AE_desired, frame_A, frame_E, params) that tracks frame E's pose in frame A. More...