centroidal_model.cc
Go to the documentation of this file.
1 /******************************************************************************
2 Copyright (c) 2018, Alexander W. Winkler. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without
5 modification, are permitted provided that the following conditions are met:
6 
7 * Redistributions of source code must retain the above copyright notice, this
8  list of conditions and the following disclaimer.
9 
10 * Redistributions in binary form must reproduce the above copyright notice,
11  this list of conditions and the following disclaimer in the documentation
12  and/or other materials provided with the distribution.
13 
14 * Neither the name of the copyright holder nor the names of its
15  contributors may be used to endorse or promote products derived from
16  this software without specific prior written permission.
17 
18 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 ******************************************************************************/
29 
32 
33 namespace towr {
34 
35 // some Eigen helper functions
36 static Eigen::Matrix3d BuildInertiaTensor( double Ixx, double Iyy, double Izz,
37  double Ixy, double Ixz, double Iyz)
38 {
39  Eigen::Matrix3d I;
40  I << Ixx, -Ixy, -Ixz,
41  -Ixy, Iyy, -Iyz,
42  -Ixz, -Iyz, Izz;
43  return I;
44 }
45 
46 // builds a cross product matrix out of "in", so in x v = X(in)*v
48 Cross(const Eigen::Vector3d& in)
49 {
50  CentroidalModel::Jac out(3,3);
51 
52  out.coeffRef(0,1) = -in(2); out.coeffRef(0,2) = in(1);
53  out.coeffRef(1,0) = in(2); out.coeffRef(1,2) = -in(0);
54  out.coeffRef(2,0) = -in(1); out.coeffRef(2,1) = in(0);
55 
56  return out;
57 }
58 
60  double Ixx, double Iyy, double Izz,
61  double Ixy, double Ixz, double Iyz,
62  int ee_count)
63  : CentroidalModel(mass,
64  BuildInertiaTensor(Ixx, Iyy, Izz, Ixy, Ixz, Iyz),
65  ee_count)
66 {
67 }
68 
69 CentroidalModel::CentroidalModel (double mass, const Eigen::Matrix3d& inertia_b,
70  int ee_count)
71  :DynamicModel(mass, ee_count)
72 {
73  I_b = inertia_b.sparseView();
74 }
75 
78 {
79  // https://en.wikipedia.org/wiki/Newton%E2%80%93Euler_equations
80 
81  Vector3d f_sum, tau_sum;
82  f_sum.setZero(); tau_sum.setZero();
83 
84  for (int ee=0; ee<ee_pos_.size(); ++ee) {
85  Vector3d f = ee_force_.at(ee);
86  tau_sum += f.cross(com_pos_ - ee_pos_.at(ee));
87  f_sum += f;
88  }
89 
90  // express inertia matrix in world frame based on current body orientation
91  Jac I_w = w_R_b_.sparseView() * I_b * w_R_b_.transpose().sparseView();
92 
93  BaseAcc acc;
94  acc.segment(AX, k3D) = I_w*omega_dot_
95  + Cross(omega_)*(I_w*omega_)
96  - tau_sum;
97  acc.segment(LX, k3D) = m()*com_acc_
98  - f_sum
99  - Vector3d(0.0, 0.0, -m()*g()); // gravity force
100  return acc;
101 }
102 
105  const Jac& jac_acc_base_lin) const
106 {
107  // build the com jacobian
108  int n = jac_pos_base_lin.cols();
109 
110  Jac jac_tau_sum(k3D, n);
111  for (const Vector3d& f : ee_force_) {
112  Jac jac_tau = Cross(f)*jac_pos_base_lin;
113  jac_tau_sum += jac_tau;
114  }
115 
116  Jac jac(k6D, n);
117  jac.middleRows(AX, k3D) = -jac_tau_sum;
118  jac.middleRows(LX, k3D) = m()*jac_acc_base_lin;
119 
120  return jac;
121 }
122 
125  double t) const
126 {
127  Jac I_w = w_R_b_.sparseView() * I_b * w_R_b_.transpose().sparseView();
128 
129  // Derivative of R*I_b*R^T * wd
130  // 1st term of product rule (derivative of R)
131  Vector3d v11 = I_b*w_R_b_.transpose()*omega_dot_;
132  Jac jac11 = base_euler.DerivOfRotVecMult(t, v11, false);
133 
134  // 2nd term of product rule (derivative of R^T)
135  Jac jac12 = w_R_b_.sparseView()*I_b*base_euler.DerivOfRotVecMult(t, omega_dot_, true);
136 
137  // 3rd term of product rule (derivative of wd)
138  Jac jac_ang_acc = base_euler.GetDerivOfAngAccWrtEulerNodes(t);
139  Jac jac13 = I_w * jac_ang_acc;
140  Jac jac1 = jac11 + jac12 + jac13;
141 
142 
143  // Derivative of w x Iw
144  // w x d_dn(R*I_b*R^T*w) -(I*w x d_dnw)
145  // right derivative same as above, just with velocity instead acceleration
146  Vector3d v21 = I_b*w_R_b_.transpose()*omega_;
147  Jac jac21 = base_euler.DerivOfRotVecMult(t, v21, false);
148 
149  // 2nd term of product rule (derivative of R^T)
150  Jac jac22 = w_R_b_.sparseView()*I_b*base_euler.DerivOfRotVecMult(t, omega_, true);
151 
152  // 3rd term of product rule (derivative of omega)
153  Jac jac_ang_vel = base_euler.GetDerivOfAngVelWrtEulerNodes(t);
154  Jac jac23 = I_w * jac_ang_vel;
155 
156  Jac jac2 = Cross(omega_)*(jac21+jac22+jac23) - Cross(I_w*omega_)*jac_ang_vel;
157 
158 
159  // Combine the two to get sensitivity to I_w*w + w x (I_w*w)
160  int n = jac_ang_vel.cols();
161  Jac jac(k6D, n);
162  jac.middleRows(AX, k3D) = jac1 + jac2;
163 
164  return jac;
165 }
166 
168 CentroidalModel::GetJacobianWrtForce (const Jac& jac_force, EE ee) const
169 {
170  Vector3d r = com_pos_ - ee_pos_.at(ee);
171  Jac jac_tau = -Cross(r)*jac_force;
172 
173  int n = jac_force.cols();
174  Jac jac(k6D, n);
175  jac.middleRows(AX, k3D) = -jac_tau;
176  jac.middleRows(LX, k3D) = -jac_force;
177 
178  return jac;
179 }
180 
182 CentroidalModel::GetJacobianWrtEEPos (const Jac& jac_ee_pos, EE ee) const
183 {
184  Vector3d f = ee_force_.at(ee);
185  Jac jac_tau = Cross(f)*(-jac_ee_pos);
186 
187  Jac jac(k6D, jac_tau.cols());
188  jac.middleRows(AX, k3D) = -jac_tau;
189 
190  // linear dynamics don't depend on endeffector position.
191  return jac;
192 }
193 
194 } /* namespace towr */
Vector3d com_acc_
x-y-z acceleration of the Center-of-Mass.
static constexpr int k6D
Matrix3d w_R_b_
rotation matrix from base (b) to world (w) frame.
virtual Jac GetJacobianWrtEEPos(const Jac &jac_ee_pos, EE) const override
How the endeffector positions affect the dynamic violation.
Eigen::Matrix< double, 6, 1 > BaseAcc
Definition: dynamic_model.h:65
double g() const
Vector3d omega_dot_
angular acceleration expressed in world frame.
ComPos com_pos_
x-y-z position of the Center-of-Mass.
Eigen::Vector3d Vector3d
Definition: dynamic_model.h:61
AngVel omega_
angular velocity expressed in world frame.
Converts Euler angles and derivatives to angular quantities.
A interface for the the system dynamics of a legged robot.
Definition: dynamic_model.h:58
Jacobian GetDerivOfAngAccWrtEulerNodes(double t) const
Jacobian of the angular acceleration with respect to the Euler nodes.
EELoad ee_force_
The endeffector force expressed in world frame.
EEPos ee_pos_
The x-y-z position of each endeffector.
CentroidalModel::Jac Cross(const Eigen::Vector3d &in)
virtual Jac GetJacobianWrtBaseAng(const EulerConverter &base_angular, double t) const override
How the base orientation affects the dynamic violation.
virtual Jac GetJacobianWrtBaseLin(const Jac &jac_base_lin_pos, const Jac &jac_acc_base_lin) const override
How the base position affects the dynamic violation.
static constexpr int k3D
virtual BaseAcc GetDynamicViolation() const override
The violation of the system dynamics incurred by the current values.
Eigen::SparseMatrix< double, Eigen::RowMajor > I_b
Centroidal Dynamics model relating forces to base accelerations.
Jacobian GetDerivOfAngVelWrtEulerNodes(double t) const
Jacobian of the angular velocity with respect to the Euler nodes.
Eigen::SparseMatrix< double, Eigen::RowMajor > Jac
Definition: dynamic_model.h:66
CentroidalModel(double mass, const Eigen::Matrix3d &inertia_b, int ee_count)
Constructs a specific Centroidal model.
Jacobian DerivOfRotVecMult(double t, const Vector3d &v, bool inverse) const
Returns the derivative of result of the linear equation M*v.
virtual Jac GetJacobianWrtForce(const Jac &jac_force, EE) const override
How the endeffector forces affect the dynamic violation.
double m() const
static Eigen::Matrix3d BuildInertiaTensor(double Ixx, double Iyy, double Izz, double Ixy, double Ixz, double Iyz)


towr_core
Author(s): Alexander W. Winkler
autogenerated on Sat Apr 7 2018 02:15:57