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: dmMobileBaseLink.hpp
21 * Author: Scott McMillan
22 * Summary: Class definition for a 6dof link (used to be the
23 * dmDynamicRefMember class.
24 *****************************************************************************/
25
26 #ifndef _DM_MOBILE_BASE_LINK_HPP
27 #define _DM_MOBILE_BASE_LINK_HPP
28
29 #include <dm.h>
30 #include <dmRigidBody.hpp>
31
32 //======================================================================
33
34 /**
35
36 The {\tt dmMobileBaseLinks} objects are links with six degrees of freedom, and
37 hence, the class is derived from the {\tt dmRigidBody} (for the dynamic
38 parameters) and, through that, the {\tt dmLink} base class (required by {\tt
39 dmArticulation} for Articulated-Body simulation functions. The default
40 constructor instantiates a generic mobile base with some valid inertial
41 parameters and zero state variables, but subsequent calls to this and the {\tt
42 dmRigidBody} member functions are needed to set the desired values.
43
44 Note that joint inputs and friction are currently ignored for this type of
45 link.
46
47 Nearly all of the remainder of the functions are described in the {\tt
48 dmLink} reference pages and are implemented in this class for the specific
49 case of links with a full 6 DOF.
50
51 See also: {\tt dmRigidBody}, {\tt dmLink}, {\tt dmLoadFile\_dm} */
52
53 class DM_DLL_API dmMobileBaseLink : public dmRigidBody
54 {
55 public:
56 enum {NUM_VARS = 7};
57
58 public:
59 ///
60 dmMobileBaseLink();
61 ///
62 virtual ~dmMobileBaseLink();
63
64 ///
getNumDOFs() const65 virtual int getNumDOFs() const { return NUM_VARS; }
66 ///
67 virtual void setState(Float q[], Float qd[]);
68 ///
69 virtual void getState(Float q[], Float qd[]) const;
70 ///
71 virtual void getPose(RotationMatrix R, CartesianVector p) const;
72
73 ///
setJointInput(Float[])74 void setJointInput(Float[]) {};
75
76 ///
77 void getDeriv(Float rdy[13]);
78
79 // Link-to-Link transformation functions:
80 /// *
81 void rtxToInboard(const CartesianVector curr, CartesianVector prev) const;
82 /// *
83 void rtxFromInboard(const CartesianVector prev, CartesianVector curr) const;
84 /// not implemented
85 void stxToInboard(const SpatialVector curr, SpatialVector prev) const;
86 /// not implemented
87 void stxFromInboard(const SpatialVector prev, SpatialVector curr) const;
88 /// not implemented
89 void rcongtxToInboardSym(const CartesianTensor Curr,
90 CartesianTensor Prev) const;
91 /// not implemented
92 void rcongtxToInboardGen(const CartesianTensor Curr,
93 CartesianTensor Prev) const;
94 /// not implemented
95 void scongtxToInboardIrefl(const SpatialTensor N, SpatialTensor I) const;
96 ///
97 void XikToInboard(Float **Xik_curr,
98 Float **Xik_prev,
99 int columns_Xik) const;
100 ///
101 void BToInboard(Float **Bkn,
102 Float **Xik, int cols_Xik,
103 Float **Xin, int cols_Xin) const;
104 ///
105 void xformZetak(Float *zetak,
106 Float **Xik, int cols_Xik) const;
107
108 // Articulated-Body (AB) algorithm functions:
109 ///
110 virtual void ABForwardKinematics(Float q[],
111 Float qd[],
112 const dmABForKinStruct &link_val_inboard,
113 dmABForKinStruct &link_val_curr);
114
115 ///
116 virtual void ABBackwardDynamics(const dmABForKinStruct &link_val_curr,
117 SpatialVector f_star_curr,
118 SpatialTensor N_refl_curr,
119 SpatialVector f_star_inboard,
120 SpatialTensor N_refl_inboard);
121 ///
122 virtual void ABBackwardDynamicsN(const dmABForKinStruct &link_val_curr,
123 SpatialVector f_star_inboard,
124 SpatialTensor N_refl_inboard);
125
126 ///
127 virtual void ABForwardAccelerations(SpatialVector a_inboard,
128 SpatialVector a_curr,
129 Float qd[],
130 Float qdd[]);
131 ///
132 void ABForwardAccelerations(SpatialVector a_inboard,
133 unsigned int *LB,
134 unsigned int num_elements_LB,
135 Float ***Xik,
136 Float **constraint_forces,
137 unsigned int *num_constraints,
138 SpatialVector a_curr,
139 Float qd[],
140 Float qdd[]);
141
142 // rendering functions:
143 ///
144 void draw() const;
145
146 private:
147 // not implemented
148 dmMobileBaseLink(const dmMobileBaseLink &);
149 dmMobileBaseLink &operator=(const dmMobileBaseLink &);
150
initABVars()151 void initABVars() {};
152 inline void setJointPos(Quaternion q, CartesianVector p);
153
154 private:
155 Quaternion m_quat; // orientation quaternion
156 // rotation matrix, ^i{\bf R}_{i-1} and
157 // position stored in dmLink::m_p
158
159 SpatialVector m_vel; // velocity state variables
160 //SpatialVector m_acc; // rate of change of velocity state variables
161 };
162
163 //----------------------------------------------------------------------------
164 // Summary: 3D rotational transformation from link CS to inboard CS
165 // Parameters: curr - 3-vector expressed in link CS
166 // Returns: prev - 3-vector expressed in inboard link CS
167 //----------------------------------------------------------------------------
rtxToInboard(const CartesianVector curr,CartesianVector prev) const168 inline void dmMobileBaseLink::rtxToInboard(const CartesianVector curr,
169 CartesianVector prev) const
170 {
171 prev[0] = m_R[0][0]*curr[0] + m_R[1][0]*curr[1] + m_R[2][0]*curr[2];
172 prev[1] = m_R[0][1]*curr[0] + m_R[1][1]*curr[1] + m_R[2][1]*curr[2];
173 prev[2] = m_R[0][2]*curr[0] + m_R[1][2]*curr[1] + m_R[2][2]*curr[2];
174 }
175
176 //----------------------------------------------------------------------------
177 // Summary: 3D rotational transformation from inboard link CS to link CS
178 // Parameters: prev - 3-vector expressed in the inboard link CS
179 // Returns: curr - 3-vector expressed in the link CS
180 //----------------------------------------------------------------------------
rtxFromInboard(const CartesianVector prev,CartesianVector curr) const181 inline void dmMobileBaseLink::rtxFromInboard(const CartesianVector prev,
182 CartesianVector curr) const
183 {
184 curr[0] = m_R[0][0]*prev[0] + m_R[0][1]*prev[1] + m_R[0][2]*prev[2];
185 curr[1] = m_R[1][0]*prev[0] + m_R[1][1]*prev[1] + m_R[1][2]*prev[2];
186 curr[2] = m_R[2][0]*prev[0] + m_R[2][1]*prev[1] + m_R[2][2]*prev[2];
187 }
188
189 #endif
190