DDP problem of CoM-ZMP model. More...

#include <DdpZmp.h>

Inheritance diagram for CCC::DdpZmp::DdpProblem:

Collaboration diagram for CCC::DdpZmp::DdpProblem:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	DdpProblem (double horizon_dt, double mass, const WeightParam &weight_param)
	Constructor. More...

void	setRefDataFunc (const std::function< RefData(double)> &ref_data_func)
	Set function of reference data. More...

virtual StateDimVector	stateEq (double t, const StateDimVector &x, const InputDimVector &u) const override
	Calculate discrete state equation. More...

virtual double	runningCost (double t, const StateDimVector &x, const InputDimVector &u) const override
	Calculate running cost. More...

virtual double	terminalCost (double t, const StateDimVector &x) const override
	Calculate terminal cost. More...

virtual void	calcStateEqDeriv (double t, const StateDimVector &x, const InputDimVector &u, Eigen::Ref< StateStateDimMatrix > state_eq_deriv_x, Eigen::Ref< StateInputDimMatrix > state_eq_deriv_u) const override
	Calculate first-order derivatives of discrete state equation. More...

virtual void	calcStateEqDeriv (double, const StateDimVector &, const InputDimVector &, Eigen::Ref< StateStateDimMatrix >, Eigen::Ref< StateInputDimMatrix >, std::vector< StateStateDimMatrix > &, std::vector< InputInputDimMatrix > &, std::vector< StateInputDimMatrix > &) const override
	Calculate first-order and second-order derivatives of discrete state equation. More...

virtual void	calcRunningCostDeriv (double t, const StateDimVector &x, const InputDimVector &u, Eigen::Ref< StateDimVector > running_cost_deriv_x, Eigen::Ref< InputDimVector > running_cost_deriv_u) const override
	Calculate first-order derivatives of running cost. More...

virtual void	calcRunningCostDeriv (double t, const StateDimVector &x, const InputDimVector &u, Eigen::Ref< StateDimVector > running_cost_deriv_x, Eigen::Ref< InputDimVector > running_cost_deriv_u, Eigen::Ref< StateStateDimMatrix > running_cost_deriv_xx, Eigen::Ref< InputInputDimMatrix > running_cost_deriv_uu, Eigen::Ref< StateInputDimMatrix > running_cost_deriv_xu) const override
	Calculate first-order and second-order derivatives of running cost. More...

virtual void	calcTerminalCostDeriv (double t, const StateDimVector &x, Eigen::Ref< StateDimVector > terminal_cost_deriv_x) const override
	Calculate first-order derivatives of terminal cost. More...

virtual void	calcTerminalCostDeriv (double t, const StateDimVector &x, Eigen::Ref< StateDimVector > terminal_cost_deriv_x, Eigen::Ref< StateStateDimMatrix > terminal_cost_deriv_xx) const override
	Calculate first-order and second-order derivatives of terminal cost. More...

Protected Attributes
double	mass_
	Robot mass [Kg]. More...

WeightParam	weight_param_
	Weight parameter. More...

std::function< RefData(double)>	ref_data_func_
	Function of reference data. More...

Detailed Description

DDP problem of CoM-ZMP model.

Dynamics is expressed by the following equation.

\begin{align*} \begin{bmatrix} \ddot{c}_x \\ \ddot{c}_y \\ \ddot{c}_z \end{bmatrix} = \begin{bmatrix} \dfrac{(c_x - z_x) f_z}{m (c_z - z_z)} \\ \dfrac{(c_y - z_y) f_z}{m (c_z - z_z)} \\ \dfrac{f_z}{m} - g \end{bmatrix} \end{align*}

\(\boldsymbol{c}\) and \(\boldsymbol{z}\) are CoM and ZMP.

State and control input are expressed by the following equations.

\begin{align*} \boldsymbol{x} = \begin{bmatrix} c_x \\ \dot{c}_x \\ c_y \\ \dot{c}_y \\ c_z \\ \dot{c}_z \end{bmatrix}, \boldsymbol{u} = \begin{bmatrix} z_x \\ z_y \\ f_z \end{bmatrix} \end{align*}

Definition at line 101 of file DdpZmp.h.

Constructor & Destructor Documentation

◆ DdpProblem()

EIGEN_MAKE_ALIGNED_OPERATOR_NEW CCC::DdpZmp::DdpProblem::DdpProblem	(	double	horizon_dt,
		double	mass,
		const WeightParam &	weight_param
	)

inline

Constructor.

Parameters

horizon_dt	discretization timestep in horizon [sec]
mass	robot mass [Kg]
weight_param	objective weight parameter

Definition at line 111 of file DdpZmp.h.

Member Function Documentation

◆ calcRunningCostDeriv() [1/2]

virtual void CCC::DdpZmp::DdpProblem::calcRunningCostDeriv	(	double	t,
		const StateDimVector &	x,
		const InputDimVector &	u,
		Eigen::Ref< StateDimVector >	running_cost_deriv_x,
		Eigen::Ref< InputDimVector >	running_cost_deriv_u
	)		const

overridevirtual

Calculate first-order derivatives of running cost.

Parameters

t	time [sec]
x	state
u	input
running_cost_deriv_x	first-order derivative of running cost w.r.t. state
running_cost_deriv_u	first-order derivative of running cost w.r.t. input

◆ calcRunningCostDeriv() [2/2]

virtual void CCC::DdpZmp::DdpProblem::calcRunningCostDeriv	(	double	t,
		const StateDimVector &	x,
		const InputDimVector &	u,
		Eigen::Ref< StateDimVector >	running_cost_deriv_x,
		Eigen::Ref< InputDimVector >	running_cost_deriv_u,
		Eigen::Ref< StateStateDimMatrix >	running_cost_deriv_xx,
		Eigen::Ref< InputInputDimMatrix >	running_cost_deriv_uu,
		Eigen::Ref< StateInputDimMatrix >	running_cost_deriv_xu
	)		const

overridevirtual

Calculate first-order and second-order derivatives of running cost.

Parameters

t	time [sec]
x	state
u	input
running_cost_deriv_x	first-order derivative of running cost w.r.t. state
running_cost_deriv_u	first-order derivative of running cost w.r.t. input
running_cost_deriv_xx	second-order derivative of running cost w.r.t. state
running_cost_deriv_uu	second-order derivative of running cost w.r.t. input
running_cost_deriv_xu	second-order derivative of running cost w.r.t. state and input

◆ calcStateEqDeriv() [1/2]

virtual void CCC::DdpZmp::DdpProblem::calcStateEqDeriv	(	double	t,
		const StateDimVector &	x,
		const InputDimVector &	u,
		Eigen::Ref< StateStateDimMatrix >	state_eq_deriv_x,
		Eigen::Ref< StateInputDimMatrix >	state_eq_deriv_u
	)		const

overridevirtual

Calculate first-order derivatives of discrete state equation.

Parameters

t	time [sec]
x	state
u	input
state_eq_deriv_x	first-order derivative of state equation w.r.t. state
state_eq_deriv_u	first-order derivative of state equation w.r.t. input

◆ calcStateEqDeriv() [2/2]

virtual void CCC::DdpZmp::DdpProblem::calcStateEqDeriv	(	double	,
		const StateDimVector &	,
		const InputDimVector &	,
		Eigen::Ref< StateStateDimMatrix >	,
		Eigen::Ref< StateInputDimMatrix >	,
		std::vector< StateStateDimMatrix > &	,
		std::vector< InputInputDimMatrix > &	,
		std::vector< StateInputDimMatrix > &
	)		const

inlineoverridevirtual

Calculate first-order and second-order derivatives of discrete state equation.

Parameters

t	time [sec]
x	state
u	input
state_eq_deriv_x	first-order derivative of state equation w.r.t. state
state_eq_deriv_u	first-order derivative of state equation w.r.t. input
state_eq_deriv_xx	second-order derivative of state equation w.r.t. state
state_eq_deriv_uu	second-order derivative of state equation w.r.t. input
state_eq_deriv_xu	second-order derivative of state equation w.r.t. state and input

Definition at line 170 of file DdpZmp.h.

◆ calcTerminalCostDeriv() [1/2]

virtual void CCC::DdpZmp::DdpProblem::calcTerminalCostDeriv	(	double	t,
		const StateDimVector &	x,
		Eigen::Ref< StateDimVector >	terminal_cost_deriv_x
	)		const

overridevirtual

Calculate first-order derivatives of terminal cost.

Parameters

t	time [sec]
x	state
terminal_cost_deriv_x	first-order derivative of terminal cost w.r.t. state

◆ calcTerminalCostDeriv() [2/2]

virtual void CCC::DdpZmp::DdpProblem::calcTerminalCostDeriv	(	double	t,
		const StateDimVector &	x,
		Eigen::Ref< StateDimVector >	terminal_cost_deriv_x,
		Eigen::Ref< StateStateDimMatrix >	terminal_cost_deriv_xx
	)		const

overridevirtual

Calculate first-order and second-order derivatives of terminal cost.

Parameters

t	time [sec]
x	state
terminal_cost_deriv_x	first-order derivative of terminal cost w.r.t. state
terminal_cost_deriv_xx	second-order derivative of terminal cost w.r.t. state

◆ runningCost()

virtual double CCC::DdpZmp::DdpProblem::runningCost	(	double	t,
		const StateDimVector &	x,
		const InputDimVector &	u
	)		const

overridevirtual

Calculate running cost.

Parameters

t	time [sec]
x	current state (x[k])
u	current input (u[k])

Returns: running cost (L[k])

◆ setRefDataFunc()

void CCC::DdpZmp::DdpProblem::setRefDataFunc ( const std::function< RefData(double)> & ref_data_func )

inline

Set function of reference data.

Parameters

ref_data_func function of reference data

Definition at line 119 of file DdpZmp.h.

◆ stateEq()

virtual StateDimVector CCC::DdpZmp::DdpProblem::stateEq	(	double	t,
		const StateDimVector &	x,
		const InputDimVector &	u
	)		const

overridevirtual

Calculate discrete state equation.

Parameters

t	time [sec]
x	current state (x[k])
u	current input (u[k])

Returns: next state (x[k+1])

◆ terminalCost()

virtual double CCC::DdpZmp::DdpProblem::terminalCost	(	double	t,
		const StateDimVector &	x
	)		const

overridevirtual

Calculate terminal cost.

Parameters

t	time [sec]
x	current state (x[k])

Returns: terminal cost (phi[k])

Member Data Documentation

◆ mass_

double CCC::DdpZmp::DdpProblem::mass_

protected

Robot mass [Kg].

Definition at line 237 of file DdpZmp.h.

◆ ref_data_func_

std::function<RefData(double)> CCC::DdpZmp::DdpProblem::ref_data_func_

protected

Function of reference data.

Definition at line 243 of file DdpZmp.h.

◆ weight_param_

WeightParam CCC::DdpZmp::DdpProblem::weight_param_

protected

Weight parameter.

Definition at line 240 of file DdpZmp.h.

The documentation for this class was generated from the following file:

/home/runner/work/CentroidalControlCollection/CentroidalControlCollection/catkin_ws/src/CentroidalControlCollection/include/CCC/DdpZmp.h

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ DdpProblem()

Member Function Documentation

◆ calcRunningCostDeriv() [1/2]

◆ calcRunningCostDeriv() [2/2]

◆ calcStateEqDeriv() [1/2]

◆ calcStateEqDeriv() [2/2]

◆ calcTerminalCostDeriv() [1/2]

◆ calcTerminalCostDeriv() [2/2]

◆ runningCost()

◆ setRefDataFunc()

◆ stateEq()

◆ terminalCost()

Member Data Documentation

◆ mass_

◆ ref_data_func_

◆ weight_param_