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.
361 lines
10 KiB
361 lines
10 KiB
#pragma once
|
|
|
|
#include "drake/common/drake_assert.h"
|
|
#include "drake/common/drake_copyable.h"
|
|
#include "drake/common/drake_deprecated.h"
|
|
#include "drake/common/trajectories/exponential_plus_piecewise_polynomial.h"
|
|
#include "drake/common/trajectories/piecewise_polynomial.h"
|
|
|
|
namespace drake {
|
|
namespace systems {
|
|
namespace controllers {
|
|
|
|
/**
|
|
Given a desired two dimensional (X and Y) zero-moment point (ZMP) trajectory
|
|
parameterized as a piecewise polynomial, an optimal center of mass (CoM)
|
|
trajectory is planned using a linear inverted pendulum model (LIPM).
|
|
A second order value function (optimal cost-to-go) and a linear policy are
|
|
also computed alongside the optimal trajectory.
|
|
The system dynamics for the X and Y directions are decoupled, however, we
|
|
plan the XY motion together for convenience.
|
|
|
|
Let \f$ c \f$ be the CoM position, the state of the system, \f$ x \f$,
|
|
is \f$ [c; \dot{c}] \f$, the control, \f$ u = \ddot{c} \f$,
|
|
and \f$ y \f$ represents the center of pressure (CoP).
|
|
For the X direction, the LIPM dynamics is:
|
|
\f[
|
|
y = c - \frac{z}{g} * u,
|
|
\f]
|
|
where \f$ g \f$ is the gravity constant and \f$ z \f$ is the CoM height.
|
|
\f$ z \f$ is assumed to be constant in LIPM.
|
|
The full dynamics can also be written in the matrix form as:
|
|
\f[
|
|
\dot{x} = A x + B u \\
|
|
y = C x + D u
|
|
\f]
|
|
|
|
The one step cost function \f$ L \f$ is defined as:
|
|
\f[
|
|
L(y, u, t) = (y - y_d(t))^T Q_y (y - y_d(t)) + u^T R u,
|
|
\f]
|
|
where \f$ Q_y \f$ and \f$ R \f$ are weighting matrices, and \f$ y_d(t) \f$
|
|
is the desired ZMP trajectory at time \f$ t \f$.
|
|
|
|
The value function is defined as
|
|
\f[
|
|
V(x, t) = \min_{u[t:t_f]} \bar{x}(t_f)^T S \bar{x}(t_f)
|
|
+ \int_{t}^{t_f} L(y, u, \tau) d\tau,
|
|
\f]
|
|
subject to the dynamics, and \f$ t_f \f$ is the last time in the desired
|
|
ZMP trajectory, \f$ \bar{x} = [c - y_d(t_f); \dot{c}] \f$,
|
|
\f$ S \f$ is the quadratic term from the infinite horizon continuous time
|
|
LQR solution solved with the same dynamics and one step cost function.
|
|
|
|
For this problem, \f$ V \f$ is known to have a quadratic form of:
|
|
\f[
|
|
V(x, t) = \bar{x}^T V_{xx} \bar{x} + \bar{x}^T V_x(t) + V_0(t),
|
|
\f]
|
|
and the corresponding optimal control policy, \f$ u^* \f$, is linear
|
|
w.r.t. to \f$ x \f$:
|
|
\f[
|
|
u^*(x, t) = K \bar{x} + u_0(t).
|
|
\f]
|
|
|
|
See the following reference for more details about the algorithm:
|
|
|
|
[1] R. Tedrake, S. Kuindersma, R. Deits and K. Miura, "A closed-form solution
|
|
for real-time ZMP gait generation and feedback stabilization,"
|
|
2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids),
|
|
Seoul, 2015, pp. 936-940.
|
|
|
|
@ingroup planning_trajectory
|
|
*/
|
|
class ZmpPlanner {
|
|
public:
|
|
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ZmpPlanner)
|
|
|
|
ZmpPlanner() = default;
|
|
|
|
/**
|
|
Implements the algorithm in [1] that computes a nominal CoM trajectory,
|
|
and the corresponding second order value function and linear policy.
|
|
|
|
No other public method should be called until Plan() has been called.
|
|
|
|
The velocity of the ZMP at the end of `zmp_d` does not need to be zero, but
|
|
the user should treat the result with caution, since the resulting nominal
|
|
CoM trajectory diverges exponentially quickly beyond the end of `zmp_d`.
|
|
@param zmp_d Desired two dimensional ZMP trajectory.
|
|
@param x0 Initial CoM state.
|
|
@param height CoM height from the ground.
|
|
@param gravity Gravity constant, defaults to 9.81
|
|
@param Qy Quadratic cost term on ZMP deviation from the desired.
|
|
@param R Quadratic cost term on CoM acceleration.
|
|
*/
|
|
void Plan(const trajectories::PiecewisePolynomial<double>& zmp_d,
|
|
const Eigen::Vector4d& x0, double height, double gravity = 9.81,
|
|
const Eigen::Matrix2d& Qy = Eigen::Matrix2d::Identity(),
|
|
const Eigen::Matrix2d& R = Eigen::Matrix2d::Zero());
|
|
|
|
/**
|
|
Returns true if Plan() has been called.
|
|
*/
|
|
bool has_planned() const { return planned_; }
|
|
|
|
/**
|
|
Computes the optimal control (CoM acceleration) at `time` given CoM state
|
|
`x` using the linear policy.
|
|
@param time Current time.
|
|
@param x Current state.
|
|
@return Optimal CoMdd.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
Eigen::Vector2d ComputeOptimalCoMdd(double time,
|
|
const Eigen::Vector4d& x) const;
|
|
|
|
/**
|
|
Converts CoM acceleration to center of pressure (CoP) using
|
|
cop = C * x + D * u, which is equivalent to
|
|
cop = com - z / g * comdd
|
|
Should only be called after Plan is called.
|
|
@param x CoM position and velocity
|
|
@param u CoM acceleration
|
|
@return center of pressure (CoP)
|
|
@pre Plan() has already been called.
|
|
*/
|
|
Eigen::Vector2d comdd_to_cop(const Eigen::Vector4d& x,
|
|
const Eigen::Vector2d& u) const {
|
|
DRAKE_DEMAND(planned_);
|
|
return C_ * x + D_ * u;
|
|
}
|
|
|
|
/**
|
|
Getter for A matrix.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
const Eigen::Matrix<double, 4, 4>& get_A() const {
|
|
DRAKE_DEMAND(planned_);
|
|
return A_;
|
|
}
|
|
|
|
/**
|
|
Getter for B matrix.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
const Eigen::Matrix<double, 4, 2>& get_B() const {
|
|
DRAKE_DEMAND(planned_);
|
|
return B_;
|
|
}
|
|
|
|
/**
|
|
Getter for C matrix.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
const Eigen::Matrix<double, 2, 4>& get_C() const {
|
|
DRAKE_DEMAND(planned_);
|
|
return C_;
|
|
}
|
|
|
|
/**
|
|
Getter for D matrix.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
const Eigen::Matrix<double, 2, 2>& get_D() const {
|
|
DRAKE_DEMAND(planned_);
|
|
return D_;
|
|
}
|
|
|
|
/**
|
|
Getter for Qy matrix.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
const Eigen::Matrix<double, 2, 2>& get_Qy() const {
|
|
DRAKE_DEMAND(planned_);
|
|
return Qy_;
|
|
}
|
|
|
|
/**
|
|
Getter for R matrix.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
const Eigen::Matrix<double, 2, 2>& get_R() const {
|
|
DRAKE_DEMAND(planned_);
|
|
return R_;
|
|
}
|
|
|
|
/**
|
|
Returns the desired ZMP evaluated at `time`.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
Eigen::Vector2d get_desired_zmp(double time) const {
|
|
DRAKE_DEMAND(planned_);
|
|
return zmp_d_.value(time);
|
|
}
|
|
|
|
/**
|
|
Returns the nominal CoM position evaluated at `time`.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
Eigen::Vector2d get_nominal_com(double time) const {
|
|
DRAKE_DEMAND(planned_);
|
|
return com_.value(time);
|
|
}
|
|
|
|
/**
|
|
Returns the nominal CoM velocity evaluated at `time`.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
Eigen::Vector2d get_nominal_comd(double time) const {
|
|
DRAKE_DEMAND(planned_);
|
|
return comd_.value(time);
|
|
}
|
|
|
|
/**
|
|
Returns the nominal CoM acceleration evaluated at `time`.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
Eigen::Vector2d get_nominal_comdd(double time) const {
|
|
DRAKE_DEMAND(planned_);
|
|
return comdd_.value(time);
|
|
}
|
|
|
|
/**
|
|
Returns the position of the ZMP at the end of the desired trajectory.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
Eigen::Vector2d get_final_desired_zmp() const {
|
|
DRAKE_DEMAND(planned_);
|
|
return zmp_d_.value(zmp_d_.end_time());
|
|
}
|
|
|
|
/**
|
|
Returns the desired ZMP trajectory.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
const trajectories::PiecewisePolynomial<double>& get_desired_zmp() const {
|
|
DRAKE_DEMAND(planned_);
|
|
return zmp_d_;
|
|
}
|
|
|
|
/**
|
|
Returns the nominal CoM trajectory.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
const trajectories::ExponentialPlusPiecewisePolynomial<double>&
|
|
get_nominal_com() const {
|
|
DRAKE_DEMAND(planned_);
|
|
return com_;
|
|
}
|
|
|
|
/**
|
|
Returns the nominal CoM velocity trajectory.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
const trajectories::ExponentialPlusPiecewisePolynomial<double>&
|
|
get_nominal_comd() const {
|
|
DRAKE_DEMAND(planned_);
|
|
return comd_;
|
|
}
|
|
|
|
/**
|
|
Returns the nominal CoM acceleration trajectory.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
const trajectories::ExponentialPlusPiecewisePolynomial<double>&
|
|
get_nominal_comdd() const {
|
|
DRAKE_DEMAND(planned_);
|
|
return comdd_;
|
|
}
|
|
|
|
/**
|
|
Returns the time invariant second order term (S1 in [1]) of the value
|
|
function.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
const Eigen::Matrix<double, 4, 4>& get_Vxx() const {
|
|
DRAKE_DEMAND(planned_);
|
|
return S1_;
|
|
}
|
|
|
|
/**
|
|
Returns the time varying first order term (s2 in [1]) of the value function.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
const trajectories::ExponentialPlusPiecewisePolynomial<double>& get_Vx()
|
|
const {
|
|
DRAKE_DEMAND(planned_);
|
|
return s2_;
|
|
}
|
|
|
|
/**
|
|
Returns the time varying first order term (s2 in [1]) of the value function,
|
|
evaluated at the given `time`.
|
|
@pre Plan() has already been called.
|
|
*/
|
|
const Eigen::Vector4d get_Vx(double time) const {
|
|
DRAKE_DEMAND(planned_);
|
|
return s2_.value(time);
|
|
}
|
|
|
|
private:
|
|
// Check if the last point of zmp_d is stationary (first and higher
|
|
// derivatives are zero).
|
|
bool CheckStationaryEndPoint(
|
|
const trajectories::PiecewisePolynomial<double>& zmp_d) const;
|
|
|
|
// Used to test whether the last point of the desired ZMP trajectory is
|
|
// stationary or not in CheckStationaryEndPoint. This number is currently
|
|
// arbitrarily chosen.
|
|
static constexpr double kStationaryThreshold = 1e-8;
|
|
|
|
// Symbols:
|
|
// x: [com; comd]
|
|
// y: zmp
|
|
// y_tf: last zmp_d
|
|
// u: comdd
|
|
// x_bar = [x - y_tf, xd]
|
|
// y_bar = y - y_tf
|
|
|
|
// Desired ZMP trajectories.
|
|
trajectories::PiecewisePolynomial<double> zmp_d_;
|
|
|
|
// Nominal CoM, CoMd, and CoMdd trajectories.
|
|
trajectories::ExponentialPlusPiecewisePolynomial<double> com_;
|
|
trajectories::ExponentialPlusPiecewisePolynomial<double> comd_;
|
|
trajectories::ExponentialPlusPiecewisePolynomial<double> comdd_;
|
|
|
|
// System dynamics matrices.
|
|
Eigen::Matrix<double, 4, 4> A_;
|
|
Eigen::Matrix<double, 4, 2> B_;
|
|
Eigen::Matrix<double, 2, 4> C_;
|
|
Eigen::Matrix<double, 2, 2> D_;
|
|
|
|
Eigen::Matrix<double, 2, 4> NB_;
|
|
Eigen::Matrix<double, 2, 2> R1i_;
|
|
Eigen::Matrix<double, 4, 4> A2_;
|
|
Eigen::Matrix<double, 4, 2> B2_;
|
|
|
|
// One step cost function:
|
|
// L = (y - y_d)^T * Qy * (y - y_d)^T + u^T * R * u.
|
|
Eigen::Matrix<double, 2, 2> Qy_, R_;
|
|
|
|
// Value function
|
|
// V = x_bar^T * S1 * x_bar + x_bar^T * s2 + constant_term.
|
|
Eigen::Matrix<double, 4, 4> S1_;
|
|
trajectories::ExponentialPlusPiecewisePolynomial<double> s2_;
|
|
|
|
// Linear policy.
|
|
// u = K * x_bar + k2
|
|
Eigen::Matrix<double, 2, 4> K_;
|
|
trajectories::ExponentialPlusPiecewisePolynomial<double> k2_;
|
|
|
|
bool planned_{false};
|
|
};
|
|
|
|
using ZMPPlanner DRAKE_DEPRECATED("2023-06-01",
|
|
"Use ZmpPlanner instead") = ZmpPlanner;
|
|
|
|
} // namespace controllers
|
|
} // namespace systems
|
|
} // namespace drake
|