Skip to content

Commit

Permalink
Kinematic constraint autodiff2 (RobotLocomotion#11709)
Browse files Browse the repository at this point in the history 
OrientationConstraint, MinimumDistanceConstraint and DistanceConstraint can be constructed from MBP<ADS>.
  • Loading branch information
@hongkai-dai
hongkai-dai authored Jun 26, 2019
1 parent ca52cf1 commit 2bd7191
Show file tree
Hide file tree
Showing 13 changed files with 436 additions and 186 deletions.
78 changes: 54 additions & 24 deletions multibody/inverse_kinematics/distance_constraint.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,43 +22,61 @@ DistanceConstraint::DistanceConstraint(
double distance_upper)
: solvers::Constraint(1, RefFromPtrOrThrow(plant).num_positions(),
Vector1d(distance_lower), Vector1d(distance_upper)),
plant_{RefFromPtrOrThrow(plant)},
plant_context_{plant_context},
geometry_pair_{std::move(geometry_pair)} {
CheckPlantIsConnectedToSceneGraph(plant_, *plant_context_);
plant_double_{plant},
plant_context_double_{plant_context},
geometry_pair_{std::move(geometry_pair)},
plant_autodiff_{nullptr},
plant_context_autodiff_{nullptr} {
CheckPlantIsConnectedToSceneGraph(*plant_double_, *plant_context_double_);
}

template <typename T>
void DistanceConstraint::DoEvalGeneric(const Eigen::Ref<const VectorX<T>>& x,
VectorX<T>* y) const {
y->resize(num_constraints());
internal::UpdateContextConfiguration(plant_context_, plant_, x);
const auto& query_port = plant_.get_geometry_query_input_port();
DistanceConstraint::DistanceConstraint(
const multibody::MultibodyPlant<AutoDiffXd>* const plant,
SortedPair<geometry::GeometryId> geometry_pair,
systems::Context<AutoDiffXd>* plant_context, double distance_lower,
double distance_upper)
: solvers::Constraint(1, RefFromPtrOrThrow(plant).num_positions(),
Vector1d(distance_lower), Vector1d(distance_upper)),
plant_double_{nullptr},
plant_context_double_{nullptr},
geometry_pair_{std::move(geometry_pair)},
plant_autodiff_{plant},
plant_context_autodiff_{plant_context} {
CheckPlantIsConnectedToSceneGraph(*plant_autodiff_, *plant_context_autodiff_);
}

template <typename T, typename S>
void EvalDistance(const MultibodyPlant<T>& plant,
const SortedPair<geometry::GeometryId>& geometry_pair,
systems::Context<T>* context,
const Eigen::Ref<const VectorX<S>>& x, VectorX<S>* y) {
y->resize(1);
internal::UpdateContextConfiguration(context, plant, x);
const auto& query_port = plant.get_geometry_query_input_port();
const auto& query_object =
query_port.Eval<geometry::QueryObject<double>>(*plant_context_);
query_port.template Eval<geometry::QueryObject<T>>(*context);
// TODO(hongkai.dai): call the distance for the single pair instead for all
// pairs when SceneGraph provides the API.
const std::vector<geometry::SignedDistancePair<double>>
signed_distance_pairs =
query_object.ComputeSignedDistancePairwiseClosestPoints(kInf);
const std::vector<geometry::SignedDistancePair<T>> signed_distance_pairs =
query_object.ComputeSignedDistancePairwiseClosestPoints(kInf);
bool found_pair = false;
for (const auto& signed_distance_pair : signed_distance_pairs) {
if (SortedPair<geometry::GeometryId>(signed_distance_pair.id_A,
signed_distance_pair.id_B) ==
geometry_pair_) {
geometry_pair) {
found_pair = true;
const geometry::SceneGraphInspector<double>& inspector =
const geometry::SceneGraphInspector<T>& inspector =
query_object.inspector();
const geometry::FrameId frame_A_id =
inspector.GetFrameId(signed_distance_pair.id_A);
const geometry::FrameId frame_B_id =
inspector.GetFrameId(signed_distance_pair.id_B);
const Frame<double>& frameA =
plant_.GetBodyFromFrameId(frame_A_id)->body_frame();
const Frame<double>& frameB =
plant_.GetBodyFromFrameId(frame_B_id)->body_frame();
const Frame<T>& frameA =
plant.GetBodyFromFrameId(frame_A_id)->body_frame();
const Frame<T>& frameB =
plant.GetBodyFromFrameId(frame_B_id)->body_frame();
internal::CalcDistanceDerivatives(
plant_, *plant_context_, frameA, frameB,
plant, *context, frameA, frameB,
inspector.X_FG(signed_distance_pair.id_A) *
signed_distance_pair.p_ACa,
signed_distance_pair.distance, signed_distance_pair.nhat_BA_W, x,
Expand All @@ -70,14 +88,26 @@ void DistanceConstraint::DoEvalGeneric(const Eigen::Ref<const VectorX<T>>& x,
throw std::runtime_error(
"DistanceConstraint::DoEvalGeneric(): SceneGraph did not report the "
"distance between the pair of geometry (" +
std::to_string(geometry_pair_.first().get_value()) + ", " +
std::to_string(geometry_pair_.second().get_value()) + ")");
std::to_string(geometry_pair.first().get_value()) + ", " +
std::to_string(geometry_pair.second().get_value()) + ")");
}
}

template <typename T>
void DistanceConstraint::DoEvalGeneric(const Eigen::Ref<const VectorX<T>>& x,
VectorX<T>* y) const {
if (use_autodiff()) {
EvalDistance<AutoDiffXd, T>(*plant_autodiff_, geometry_pair_,
plant_context_autodiff_, x, y);
} else {
EvalDistance<double, T>(*plant_double_, geometry_pair_,
plant_context_double_, x, y);
}
}

void DistanceConstraint::DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
Eigen::VectorXd* y) const {
DoEvalGeneric<double>(x, y);
DoEvalGeneric(x, y);
}

void DistanceConstraint::DoEval(const Eigen::Ref<const AutoDiffVecXd>& x,
Expand Down
18 changes: 16 additions & 2 deletions multibody/inverse_kinematics/distance_constraint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,15 @@ class DistanceConstraint : public solvers::Constraint {
systems::Context<double>* plant_context,
double distance_lower, double distance_upper);

/**
* Overloaded constructor. Constructs the constraint with
* MultibodyPlant<AutoDiffXd>.
*/
DistanceConstraint(const multibody::MultibodyPlant<AutoDiffXd>* const plant,
SortedPair<geometry::GeometryId> geometry_pair,
systems::Context<AutoDiffXd>* plant_context,
double distance_lower, double distance_upper);

~DistanceConstraint() override {}

private:
Expand All @@ -53,9 +62,14 @@ class DistanceConstraint : public solvers::Constraint {
void DoEvalGeneric(const Eigen::Ref<const VectorX<T>>& x,
VectorX<T>* y) const;

const multibody::MultibodyPlant<double>& plant_;
systems::Context<double>* const plant_context_;
bool use_autodiff() const { return plant_autodiff_; }

const MultibodyPlant<double>* plant_double_;
systems::Context<double>* const plant_context_double_;
SortedPair<geometry::GeometryId> geometry_pair_;

const MultibodyPlant<AutoDiffXd>* const plant_autodiff_;
systems::Context<AutoDiffXd>* plant_context_autodiff_;
};
} // namespace multibody
} // namespace drake
49 changes: 19 additions & 30 deletions multibody/inverse_kinematics/distance_constraint_utilities.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,16 +5,6 @@
namespace drake {
namespace multibody {
namespace internal {
void CalcDistanceDerivatives(const MultibodyPlant<double>&,
const systems::Context<double>&,
const Frame<double>&, const Frame<double>&,
const Eigen::Vector3d&, double distance,
const Eigen::Vector3d&,
const Eigen::Ref<const AutoDiffVecXd>&,
double* distance_double) {
*distance_double = distance;
}

void CalcDistanceDerivatives(const MultibodyPlant<double>& plant,
const systems::Context<double>& context,
const Frame<double>& frameA,
Expand Down Expand Up @@ -52,26 +42,25 @@ void CalcDistanceDerivatives(const MultibodyPlant<double>& plant,
ddistance_dq * math::autoDiffToGradientMatrix(q);
}

void CheckPlantIsConnectedToSceneGraph(
const MultibodyPlant<double>& plant,
const systems::Context<double>& plant_context) {
if (!plant.geometry_source_is_registered()) {
throw std::invalid_argument(
"Kinematic constraint: MultibodyPlant has not registered "
"with a SceneGraph yet. Please refer to "
"AddMultibodyPlantSceneGraph on how to connect MultibodyPlant to "
"SceneGraph.");
}
const auto& query_port = plant.get_geometry_query_input_port();
if (!query_port.HasValue(plant_context)) {
throw std::invalid_argument(
"Kinematic constraint: Cannot get a valid "
"geometry::QueryObject. Either the plant's geometry query input port "
"is not properly connected to the SceneGraph's geometry query output "
"port, or the plant_context_ is incorrect. Please refer to "
"AddMultibodyPlantSceneGraph on connecting MultibodyPlant to "
"SceneGraph.");
}
void CalcDistanceDerivatives(const MultibodyPlant<AutoDiffXd>&,
const systems::Context<AutoDiffXd>&,
const Frame<AutoDiffXd>&, const Frame<AutoDiffXd>&,
const Vector3<AutoDiffXd>&,
const AutoDiffXd& distance_autodiff,
const Vector3<AutoDiffXd>&,
const Eigen::Ref<const Eigen::VectorXd>&,
double* distance) {
*distance = distance_autodiff.value();
}

void CalcDistanceDerivatives(const MultibodyPlant<double>&,
const systems::Context<double>&,
const Frame<double>&, const Frame<double>&,
const Eigen::Vector3d&, double distance_in,
const Eigen::Vector3d&,
const Eigen::Ref<const VectorX<double>>&,
double* distance_out) {
*distance_out = distance_in;
}
} // namespace internal
} // namespace multibody
Expand Down
58 changes: 46 additions & 12 deletions multibody/inverse_kinematics/distance_constraint_utilities.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ namespace internal {
* signed distance field to geometry B, expressed in the world frame.
* @param q The generalized position of the plant, it also stores the gradient
* dq / dz, where z is some other variables.
* @param[out] diatance_autodiff Containing the gradient of @p distance w.r.t
* @param[out] distance_autodiff Containing the gradient of @p distance w.r.t
* z (the same variable as shown up in the gradient of q).
*/
void CalcDistanceDerivatives(const MultibodyPlant<double>& plant,
Expand All @@ -32,23 +32,57 @@ void CalcDistanceDerivatives(const MultibodyPlant<double>& plant,
AutoDiffXd* distance_autodiff);

/**
* This is the overloaded version of Distance for the input q of double scalar
* type, it just copies @p distance to @p distance_double.
* This is the overloaded version of CalcDistanceDerivatives for plant being
* MultibodyPlant<AutoDiffXd> instead of MultibodyPlant<double>.
*/
void CalcDistanceDerivatives(const MultibodyPlant<double>&,
const systems::Context<double>&,
const Frame<double>&, const Frame<double>&,
const Eigen::Vector3d&, double distance,
const Eigen::Vector3d&,
const Eigen::Ref<const AutoDiffVecXd>&,
double* distance_double);
void CalcDistanceDerivatives(const MultibodyPlant<AutoDiffXd>& plant,
const systems::Context<AutoDiffXd>& context,
const Frame<AutoDiffXd>& frameA,
const Frame<AutoDiffXd>& frameB,
const Vector3<AutoDiffXd>& p_ACa,
const AutoDiffXd& distance_autodiff,
const Vector3<AutoDiffXd>& nhat_BA_W,
const Eigen::Ref<const Eigen::VectorXd>& q,
double* distance);

/**
* This is the overloaded version of CalcDistanceDerivatives for the input q of
* double scalar type, it just copies @p distance_in to @p distance_out.
*/
template <typename T>
void CalcDistanceDerivatives(const MultibodyPlant<T>&,
const systems::Context<T>&, const Frame<T>&,
const Frame<T>&, const Vector3<T>&, T distance_in,
const Vector3<T>&,
const Eigen::Ref<const VectorX<T>>&,
T* distance_out) {
*distance_out = distance_in;
}

/**
* Check if the plant has registered its geometry with the SceneGraph.
*/
template <typename T>
void CheckPlantIsConnectedToSceneGraph(
const MultibodyPlant<double>& plant,
const systems::Context<double>& plant_context);
const MultibodyPlant<T>& plant, const systems::Context<T>& plant_context) {
if (!plant.geometry_source_is_registered()) {
throw std::invalid_argument(
"Kinematic constraint: MultibodyPlant has not registered "
"with a SceneGraph yet. Please refer to "
"AddMultibodyPlantSceneGraph on how to connect MultibodyPlant to "
"SceneGraph.");
}
const auto& query_port = plant.get_geometry_query_input_port();
if (!query_port.HasValue(plant_context)) {
throw std::invalid_argument(
"Kinematic constraint: Cannot get a valid "
"geometry::QueryObject. Either the plant's geometry query input port "
"is not properly connected to the SceneGraph's geometry query output "
"port, or the plant_context_ is incorrect. Please refer to "
"AddMultibodyPlantSceneGraph on connecting MultibodyPlant to "
"SceneGraph.");
}
}
} // namespace internal
} // namespace multibody
} // namespace drake
Loading

0 comments on commit 2bd7191

Please sign in to comment.