forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
60 lines
2.2 KiB
60 lines
2.2 KiB
#include "drake/multibody/inverse_kinematics/kinematic_evaluator_utilities.h"
|
|
|
|
namespace drake {
|
|
namespace multibody {
|
|
namespace internal {
|
|
|
|
void UpdateContextConfiguration(drake::systems::Context<double>* context,
|
|
const MultibodyPlant<double>& plant,
|
|
const Eigen::Ref<const VectorX<double>>& q) {
|
|
DRAKE_ASSERT(context != nullptr);
|
|
if (q != plant.GetPositions(*context)) {
|
|
plant.SetPositions(context, q);
|
|
}
|
|
}
|
|
|
|
void UpdateContextConfiguration(drake::systems::Context<double>* context,
|
|
const MultibodyPlant<double>& plant,
|
|
const Eigen::Ref<const AutoDiffVecXd>& q) {
|
|
return UpdateContextConfiguration(context, plant, math::ExtractValue(q));
|
|
}
|
|
|
|
void UpdateContextConfiguration(systems::Context<AutoDiffXd>* context,
|
|
const MultibodyPlant<AutoDiffXd>& plant,
|
|
const Eigen::Ref<const AutoDiffVecXd>& q) {
|
|
DRAKE_ASSERT(context != nullptr);
|
|
if (!math::AreAutoDiffVecXdEqual(q, plant.GetPositions(*context))) {
|
|
plant.SetPositions(context, q);
|
|
}
|
|
}
|
|
|
|
void UpdateContextPositionsAndVelocities(
|
|
systems::Context<double>* context, const MultibodyPlant<double>& plant,
|
|
const Eigen::Ref<const Eigen::VectorXd>& q_v) {
|
|
DRAKE_ASSERT(context != nullptr);
|
|
if (q_v != plant.GetPositionsAndVelocities(*context)) {
|
|
plant.SetPositionsAndVelocities(context, q_v);
|
|
}
|
|
}
|
|
|
|
void UpdateContextPositionsAndVelocities(
|
|
systems::Context<double>* context, const MultibodyPlant<double>& plant,
|
|
const Eigen::Ref<const AutoDiffVecXd>& q_v) {
|
|
return UpdateContextPositionsAndVelocities(
|
|
context, plant, math::ExtractValue(q_v));
|
|
}
|
|
|
|
void UpdateContextPositionsAndVelocities(
|
|
systems::Context<AutoDiffXd>* context,
|
|
const MultibodyPlant<AutoDiffXd>& plant,
|
|
const Eigen::Ref<const AutoDiffVecXd>& q_v) {
|
|
DRAKE_ASSERT(context != nullptr);
|
|
if (!math::AreAutoDiffVecXdEqual(q_v,
|
|
plant.GetPositionsAndVelocities(*context))) {
|
|
plant.SetPositionsAndVelocities(context, q_v);
|
|
}
|
|
}
|
|
} // namespace internal
|
|
} // namespace multibody
|
|
} // namespace drake
|