center-of-mass.hpp 9.25 KB
Newer Older
1
//
jcarpent's avatar
jcarpent committed
2
// Copyright (c) 2015-2016 CNRS
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

Nicolas Mansard's avatar
Nicolas Mansard committed
18
19
20
21
22
#ifndef __se3_center_of_mass_hpp__
#define __se3_center_of_mass_hpp__

#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
jcarpent's avatar
jcarpent committed
23
#include "pinocchio/multibody/data.hpp"
24
#include "pinocchio/algorithm/kinematics.hpp"
jcarpent's avatar
jcarpent committed
25

Nicolas Mansard's avatar
Nicolas Mansard committed
26
27
namespace se3
{
28
29
30
  
  ///
  /// \brief Computes the center of mass position of a given model according to a particular joint configuration.
Valenza Florian's avatar
Valenza Florian committed
31
  ///        The result is accessible through data.com[0] for the full body com and data.com[i] for the subtree supported by joint i (expressed in the joint i frame).
32
33
34
35
36
37
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration vector (dim model.nq).
  /// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees.
  ///
38
  /// \return The center of mass position of the full rigid body system expressed in the world frame.
39
  ///
40
41
  inline const SE3::Vector3 &
  centerOfMass(const Model & model, Data & data,
42
               const Eigen::VectorXd & q,
43
               const bool computeSubtreeComs = true);
44
  
45
  ///
46
47
48
  /// \brief Computes the center of mass position and velocity of a given model according to a particular joint configuration and velocity.
  ///        The result is accessible through data.com[0], data.vcom[0] for the full body com position and velocity.
  ///        And data.com[i] and data.vcom[i] for the subtree supported by joint i (expressed in the joint i frame).
49
50
51
52
53
54
55
56
57
58
59
60
61
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration vector (dim model.nq).
  /// \param[in] v The joint velocity vector (dim model.nv).
  /// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees.
  ///
  /// \return The center of mass position of the full rigid body system expressed in the world frame.
  ///
  inline const SE3::Vector3 &
  centerOfMass(const Model & model, Data & data,
               const Eigen::VectorXd & q,
               const Eigen::VectorXd & v,
62
               const bool computeSubtreeComs = true);
63
  
64
65
  ///
  /// \brief Computes the center of mass position, velocity and acceleration of a given model according to a particular joint configuration, velocity and acceleration.
Valenza Florian's avatar
Valenza Florian committed
66
  ///        The result is accessible through data.com[0], data.vcom[0], data.acom[0] for the full body com position, velocity and acceleation.
67
68
69
70
71
72
73
74
75
  ///        And data.com[i], data.vcom[i] and data.acom[i] for the subtree supported by joint i (expressed in the joint i frame).
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration vector (dim model.nq).
  /// \param[in] v The joint velocity vector (dim model.nv).
  /// \param[in] a The joint acceleration vector (dim model.nv).
  /// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees.
  ///
76
77
78
79
80
81
82
  /// \return The center of mass position of the full rigid body system expressed in the world frame.
  ///
  inline const SE3::Vector3 &
  centerOfMass(const Model & model, Data & data,
               const Eigen::VectorXd & q,
               const Eigen::VectorXd & v,
               const Eigen::VectorXd & a,
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
               const bool computeSubtreeComs = true);
  
  ///
  /// \brief Computes the center of mass position, velocity and acceleration of a given model according to the current kinematic values contained in data and the template value parameters.
  ///        The result is accessible through data.com[0], data.vcom[0] and data.acom[0] for the full body com position and velocity.
  ///        And data.com[i] and data.vcom[i] for the subtree supported by joint i (expressed in the joint i frame).
  ///
  /// \tparam do_position Compute the center of mass position.
  /// \tparam do_velocity Compute the center of mass velocity.
  /// \tparam do_acceleration Compute the center of mass acceleration.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees.
  ///
  template<bool do_position, bool do_velocity, bool do_acceleration>
  inline void centerOfMass(const Model & model, Data & data,
                           const bool computeSubtreeComs = true);
  
  ///
  /// \brief Computes the center of mass position, velocity and acceleration of a given model according to the current kinematic values contained in data.
  ///        The result is accessible through data.com[0], data.vcom[0] and data.acom[0] for the full body com position and velocity.
  ///        And data.com[i] and data.vcom[i] for the subtree supported by joint i (expressed in the joint i frame).
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees.
  ///
  inline void centerOfMass(const Model & model, Data & data,
                           const bool computeSubtreeComs)
  { centerOfMass<true,true,true>(model,data,computeSubtreeComs); }
114
115
116
  
  ///
  /// \brief Computes both the jacobian and the the center of mass position of a given model according to a particular joint configuration.
117
  ///        The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (\sa se3::computeJacobians).
118
  ///        And data.com[i] gives the center of mass of the subtree supported by joint i (expressed in the world frame).
119
120
121
122
123
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration vector (dim model.nq).
  /// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees.
124
  /// \param[in] updateKinematics If true, the algorithm updates first the geometry of the tree. Otherwise, it uses the current kinematics stored in data.
125
126
127
  ///
  /// \return The jacobian of center of mass position of the rigid body system expressed in the world frame (matrix 3 x model.nv).
  ///
128
  inline const Data::Matrix3x &
129
130
  jacobianCenterOfMass(const Model & model, Data & data,
                       const Eigen::VectorXd & q,
131
132
                       const bool computeSubtreeComs = true,
                       const bool updateKinematics = true);
133
134
135
136
137

  /* If the CRBA has been run, then both COM and Jcom are easily available from
   * the mass matrix. Use the following methods to access them. In that case,
   * the COM subtrees (also easily available from CRBA data) are not
   * explicitely set. Use data.Ycrb[i].lever() to get them. */
138
139
140
141
142
143
144
145
  ///
  /// \brief Extracts the center of mass position from the joint space inertia matrix (also called the mass matrix).
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  ///
  /// \return The center of mass position of the rigid body system expressed in the world frame (vector 3).
  ///
146
  inline const SE3::Vector3 &
147
148
149
  getComFromCrba(const Model & model, Data & data);
  
  ///
150
151
  /// \brief Extracts both the jacobian of the center of mass (CoM), the total mass of the system and the CoM position from the joint space inertia matrix (also called the mass matrix).
  ///        The results are accessible through data.Jcom, data.mass[0] and data.com[0] and are both expressed in the world frame.
152
153
154
155
156
157
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  ///
  /// \return The jacobian of the CoM expressed in the world frame (matrix 3 x model.nv).
  ///
158
159
  /// \remark This extraction of inertial quantities is only valid for free-floating base systems.
  ///
160
  inline const Data::Matrix3x &
161
162
  getJacobianComFromCrba(const Model & model, Data & data);
  
Nicolas Mansard's avatar
Nicolas Mansard committed
163
164
} // namespace se3 

165
166
167
168
/* --- Details -------------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/algorithm/center-of-mass.hxx"
Nicolas Mansard's avatar
Nicolas Mansard committed
169
170

#endif // ifndef __se3_center_of_mass_hpp__