36 void DoubleIntegratorDynamicsSolver::AssignScene(
ScenePtr scene_in)
38 const int num_positions_in = scene_in->GetKinematicTree().GetNumControlledJoints();
39 if (debug_)
HIGHLIGHT_NAMED(
"DoubleIntegratorDynamicsSolver::AssignScene",
"Dimension: " << num_positions_in);
41 num_positions_ = num_positions_in;
42 num_velocities_ = num_positions_in;
43 num_controls_ = num_positions_in;
46 A_ = Eigen::MatrixXd::Zero(num_positions_ + num_velocities_, num_positions_ + num_velocities_);
47 A_.topRightCorner(num_velocities_, num_velocities_) = Eigen::MatrixXd::Identity(num_velocities_, num_velocities_);
49 B_ = Eigen::MatrixXd::Zero(num_positions_ + num_velocities_, num_controls_);
50 B_.bottomRightCorner(num_controls_, num_controls_) = Eigen::MatrixXd::Identity(num_controls_, num_controls_);
56 DynamicsSolver::ComputeDerivatives(Eigen::VectorXd(num_positions_ + num_velocities_), Eigen::VectorXd(num_controls_));
61 return A_ * x + B_ * u;
67 if (integrator_ != last_integrator_)
69 DynamicsSolver::ComputeDerivatives(x, u);
70 last_integrator_ = integrator_;
#define REGISTER_DYNAMICS_SOLVER_TYPE(TYPE, DERIV)
std::shared_ptr< Scene > ScenePtr
Eigen::Matrix< T, NU, 1 > ControlVector
#define HIGHLIGHT_NAMED(name, x)
Eigen::Matrix< T, NX, 1 > StateVector