1 /*****************************************************************************
2 * DynaMechs: A Multibody Dynamic Simulation Library
3 *
4 * Copyright (C) 1994-2001 Scott McMillan All Rights Reserved.
5 *
6 * This library is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License
8 * as published by the Free Software Foundation; either version 2
9 * of the License, or (at your option) any later version.
10 *
11 * This library is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 * General Public License for more details.
15 *
16 * You should have received a copy of the GNU Library General Public
17 * License along with this library; if not, write to the Free
18 * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19 *****************************************************************************
20 * File: dmSystem.hpp
21 * Author: Scott McMillan
22 * Summary: Class definitions for dmSystem - top level DynaMechs structure
23 *****************************************************************************/
24
25 #ifndef _DM_SYSTEM_HPP
26 #define _DM_SYSTEM_HPP
27
28 #include <dm.h>
29 #include <dmObject.hpp>
30
31 //======================================================================
32
33 /**
34
35 The {\tt dmSystem} is the abstract base class for all types of objects that
36 need to be simulated by a {\tt dmIntegrator} class. As such this class defines
37 the interface needed by the dmIntegrator classes, and is the base class
38 for robotic systems (dmArticulation and dmClosedArticulation), dynamic
39 environments (dmTreadmill through dmEnvironment), and (in the future) control
40 systems. By adding all the dynamic systems to a single integrator, it will be
41 ensured that all the systems will be numerically integrated in synchrony with
42 one another. This is especially important when multi-step and/or variable
43 stepsize numerical integrators are used, where the system dynamics are
44 calculated at more then one point across the interval, where some computations
45 may be rejected due to accuracy constraints, and the interval may be subdivided
46 into smaller steps.
47
48 The {\tt setState} and {\tt getState} functions are used to set and query joint
49 states in the entire tree. {\tt setState} sets the state of the DOFs in the
50 articulation. The two arguments are both packed arrays (like {\tt
51 joint\_input}) containing the joint positions and velocities of the links. The
52 reverse operation, {\tt getState}, fills packed arrays with the joint positions
53 velocities. A convenience function, {\tt getNumDOFs}, returns the total number
54 of degrees of freedom in the articulation and can be used to determine the
55 appropriate size of the above arrays.
56
57 The {\tt initSimVars} function initializes the state and derivative of state
58 vectors with the current state of the system. The parameters, {\tt qy} and
59 {\tt qdy}, are the arrays with 2*{\tt getNumDOFs()} elements. All necessary
60 calls to {\tt initSimVars} are done automatically by the {\tt dmIntegrator}
61 class when a {\tt dmSystem} object is added to or removed from it. Note that
62 {\tt initSimVars} is different from the {\tt getState} function that returns
63 just the position and velocity state vectors in two separate vectors ({\tt q}
64 and {\tt qd}) of {\tt getNumDOFs()} each. {\bf IMPORTANT}: The {\tt
65 dmIntegrator} class cannot detect subsequent changes that would affect the
66 number of DOFs (e.g., addition/removal of links); therefore, the entire system
67 must be built before it is assigned to the {\tt dmIntegrator} object and no
68 changes can be made after this point.
69
70 Specification of an inertial coordinate system other than the origin can be
71 specified by calling {\tt setRefSystem} and passing the position and
72 orientation (relative to the origin) of the new coordinate system.
73 This ability is not necessary or even computationally efficient, but it makes
74 it much more flexible in specifying systems. The functions {\tt getRefSystem}
75 and {\tt getPose} return this position and orientation -- the latter in two
76 different formats: quaternion or rotation matrix. The functions, {\tt
77 rtxFromICS} and {\tt rtxToICS}, are used to rotate free vectors from one system
78 to the other.
79
80 The {\tt dynamics} function is the entry point for computation of the dynamics
81 of the system. It takes, as input, the state vector {\tt qy} and computes the
82 derivative of state vector, {\tt qdy}. This is the derivative function that
83 the {\tt dmIntegrator} class uses to perform the numerical integration.
84
85 Two functions are provided for use by adaptive-stepsize integrators which may
86 need to back out of a rejected integration step. This is required because some
87 {\tt dmForce} objects attached to links may retain state information. The {\tt
88 pushForceStates} function is used to save the state information of all force
89 objects attached to links in the system, while the {\tt popForceStates}
90 function is used if necessary to restore the state to the stored values.
91
92 Another two functions are used for retrieving the energy of the system. The
93 {\tt getPotentialEnergy} function returns the potential energy. Note that the
94 potential energy zero is defined by the plane which passes through the origin
95 and has the gravity vector as its normal. The {\tt getKineticEnergy} function
96 returns the sum of translational and rotational kinetic energy of the system.
97
98 See also: {\tt dmArticulation, dmEnvironment, dmIntegrator}. */
99
100 //======================================================================
101
102 class DM_DLL_API dmSystem : public dmObject
103 {
104 public:
105 ///
106 dmSystem();
107 ///
108 virtual ~dmSystem();
109
110 ///
111 virtual unsigned int getNumDOFs() const = 0;
112 ///
113 virtual void setState(Float q[], Float qd[]) = 0;
114 /// qy and qdy are arrays with getNumDOFs() elements
115 virtual void getState(Float q[], Float qd[]) const = 0;
116
117 /// qy and qdy are arrays with 2*getNumDOFs() elements
118 void initSimVars(Float *qy, Float *qdy);
119
120 // transformation functions
121 ///
122 void setRefSystem(Quaternion quat, CartesianVector pos);
123 ///
124 void getRefSystem(Quaternion quat, CartesianVector pos) const;
125 ///
126 void getPose(RotationMatrix R, CartesianVector pos) const;
127
128 ///
129 inline void rtxFromICS(const CartesianVector p_ICS,
130 CartesianVector p_ref) const;
131 ///
132 inline void rtxToICS(const CartesianVector p_ref,
133 CartesianVector p_ICS) const;
134
135 ///
136 virtual void pushForceStates() = 0;
137 ///
138 virtual void popForceStates() = 0;
139 ///
140 virtual Float getPotentialEnergy() const = 0;
141 ///
142 virtual Float getKineticEnergy() const = 0;
143
144 // dynamics algorithm
145 ///
146 virtual void dynamics(Float *qy, Float *qdy) = 0;
147
148 virtual void draw() const = 0;
149
150 protected:
151 // not implemented
152 dmSystem(const dmSystem &);
153 dmSystem &operator=(const dmSystem &);
154
155 // forward kinematics helper routine
156 void forwardKinematics(dmABForKinStruct &fk) const;
157
158 protected:
159 // description of transform to reference system
160 Quaternion m_quat_ICS;
161 RotationMatrix m_R_ICS; // pose of links wrt ICS - ^{ICS}R_{i}
162 CartesianVector m_p_ICS; // pos. of links wrt ICS - ^{ICS}p_{i}
163 };
164
165 //----------------------------------------------------------------------------
166 // Summary: 3D rotational transformation from ICS to ref mem CS
167 // Parameters: p_ICS - 3-vector expressed in the ICS
168 // Returns: p_ref - 3-vector expressed in ref mem's CS
169 //----------------------------------------------------------------------------
rtxFromICS(const CartesianVector p_ICS,CartesianVector p_ref) const170 inline void dmSystem::rtxFromICS(const CartesianVector p_ICS,
171 CartesianVector p_ref) const
172 {
173 p_ref[0] = m_R_ICS[0][0]*p_ICS[0] + m_R_ICS[0][1]*p_ICS[1] +
174 m_R_ICS[0][2]*p_ICS[2];
175 p_ref[1] = m_R_ICS[1][0]*p_ICS[0] + m_R_ICS[1][1]*p_ICS[1] +
176 m_R_ICS[1][2]*p_ICS[2];
177 p_ref[2] = m_R_ICS[2][0]*p_ICS[0] + m_R_ICS[2][1]*p_ICS[1] +
178 m_R_ICS[2][2]*p_ICS[2];
179 }
180
181 //----------------------------------------------------------------------------
182 // Summary: 3D rotational transformation from ref mem CS to ICS
183 // Parameters: p_ref - 3-vector expressed in ref mem's CS
184 // Returns: p_ICS - 3-vector expressed in the ICS
185 //----------------------------------------------------------------------------
rtxToICS(const CartesianVector p_ref,CartesianVector p_ICS) const186 inline void dmSystem::rtxToICS(const CartesianVector p_ref,
187 CartesianVector p_ICS) const
188 {
189 p_ICS[0] = m_R_ICS[0][0]*p_ref[0] + m_R_ICS[1][0]*p_ref[1] +
190 m_R_ICS[2][0]*p_ref[2];
191 p_ICS[1] = m_R_ICS[0][1]*p_ref[0] + m_R_ICS[1][1]*p_ref[1] +
192 m_R_ICS[2][1]*p_ref[2];
193 p_ICS[2] = m_R_ICS[0][2]*p_ref[0] + m_R_ICS[1][2]*p_ref[1] +
194 m_R_ICS[2][2]*p_ref[2];
195 }
196
197 #endif
198