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: dmArticulation.hpp 21 * Author: Scott McMillan 22 * Summary: Class definition for general articulated structures 23 *****************************************************************************/ 24 25 #ifndef _DM_ARTICULATION_HPP 26 #define _DM_ARTICULATION_HPP 27 28 #include <dm.h> 29 #include <dmLink.hpp> 30 #include <dmSystem.hpp> 31 32 #include <vector> 33 34 //============================================================================ 35 /** 36 37 The {\tt dmArticulation} class would be better named the {\tt dmTreeStructure} 38 class. It is a subclass of the {\tt dmSystem} class that implements the 39 functionality needed to model a multibody system organized in a tree topology 40 (Note: a subclass of {\tt dmArticulation} extends this class to support 41 closed-chain structures). This class contains a database of links and the 42 predecessor/successor relationships between links that define the tree 43 topology. As such, the first link and any other link added to the articulation 44 using the {\tt addLink} function with the second argument set to NULL are 45 considered that root of the tree. Then, as additional links are added to the 46 articulation, a pointer to a parent link (already added to the articulation) is 47 used to specify its location in the tree. As a result, each link in a tree has 48 only one parent. The only exceptions are the root links that are defined 49 relative to the {\tt dmSystem}'s inertial reference coordinate system. 50 51 The default constructor returns an empty articulation (that is, with no 52 links). As links are added to the articulation, internal variables are 53 allocated that will be used to perform the dynamic computations. When the 54 destructor is called these internal variables are freed but any links that 55 have been added to the articulation are not. 56 57 The {\tt addLink} function is called to add a link to the tree structure that 58 is maintained by the {\tt dmArticulation} objects. The first parameter is a 59 pointer to a (previously instantiated) link object that is to be added, and the 60 second parameter is a pointer to a link that has already been added to the 61 articulation and will be the "parent" to the new link and placed in the 62 database accordingly. Note that the first link added has no parent so the {\tt 63 parent} parameter is set to {\tt NULL}. If the operation is successful, this 64 function returns {\tt true}; otherwise it will return {\tt false} for any one 65 of a number of reasons (parent not found, variable allocation failure, etc.) 66 67 The articulation can be queried for the number of links in the tree by calling 68 {\tt getNumLinks}. A pointer to a specific link can be obtained by calling 69 {\tt getLink} with the appropriate index. The index corresponds to the order 70 in which the links were added to the tree: 0 for the first link, 71 1 for the second, etc. If the index is out of range, {\tt getLink} returns 72 {\tt NULL}. The inverse function, {\tt getLinkIndex} takes a {\tt dmLink} 73 pointer and returns its corresponding index, or {\tt -1} if the link is not 74 contained in the tree or the pointer is {\tt NULL}. 75 76 Three functions, {\tt setJointInput}, {\tt setState}, and {\tt getState}, are 77 used to set and query joint states and inputs for the links in the entire 78 tree. The first, {\tt setJointInput}, sets the joint inputs (either torques, 79 forces, or motor voltages) for all the links in the articulation. Its {\tt 80 joint\_input} argument is a packed (one-dimensional) array with length equal 81 to the total number of DOFs in the articulation. The elements of this array 82 correspond to the DOFs of each link in the order they were added to the 83 articulation. {\tt setState} sets the state of the DOFs in the articulation. 84 The two arguments are both packed arrays (like {\tt joint\_input}) containing 85 the joint positions and velocities of the links. The reverse operation, {\tt 86 getState}, fills packed arrays with the joint positions velocities. A 87 convenience function, {\tt getNumDOFs}, returns the total number of degrees 88 of freedom in the articulation and can be used to determine the appropriate 89 size of the above arrays. 90 91 Two functions are provided to compute the forward kinematics of the 92 articulation. One {\tt forwardKinematics} function computes the homogeneous 93 transformation matrix (4x4) describing the position (last column) and 94 orientation (upper left 3x3 submatrix) with respect to the inertial coordinate 95 system of the link specified by the link index. If the link index is valid (in 96 range), the function returns {\tt true} and the result is in {\tt mat}; 97 otherwise, it returns {\tt false} and the result in {\tt mat} is unchanged. 98 The second {\tt forwardKinematics} function is called by the first, and 99 composes the result in the second parameter, {\tt fk}, which is a reference to 100 a {\tt dmABForKinStruct} (from {\tt dm.h}): 101 \begin{verbatim} 102 struct dmABForKinStruct 103 { 104 RotationMatrix R_ICS; // orientation of links wrt ICS - ^{ICS}R_{i} 105 CartesianVector p_ICS; // position of links wrt ICS - ^{ICS}p_{i} 106 SpatialVector v; // velocity of link wrt to i. 107 ... 108 }; 109 \end{verbatim} 110 Upon exit this function will fill {\tt R\_ICS} and {\tt p\_ICS} with the 111 resulting transformation information and return {\tt true}. If the {\tt 112 link\_index} is out of range, this function also returns {\tt false}. 113 114 The {\tt dynamics} function is the entry point for computation of the dynamics 115 of the system. For this class, this is a wrapper around the Articulated Body 116 (AB) dynamics computation for the multibody system. The three functions, {\tt 117 ABForwardKinematics}, {\tt ABBackwardDynamics}, and {\tt 118 ABForwardAccelerations}, comprise the implementation of the Articulated-Body 119 (AB) Simulation alogrithm recursions and are hidden from the user. 120 However, during the ABForwardKinematics traversal, {\tt dmABForKinStruct}s are 121 computed for each link which can be accessed by calling the {\tt 122 getForKinStruct} function with the index of the desired link. This functions 123 returns a pointer to the requested struct, or NULL if the index is out of 124 range. 125 126 See also {\tt dmSystem, dmLink}. 127 128 */ 129 130 //============================================================================ 131 132 class DM_DLL_API dmArticulation : public dmSystem 133 { 134 public: 135 /// 136 dmArticulation(); 137 /// 138 virtual ~dmArticulation(); 139 140 /// 141 bool addLink(dmLink *new_link, dmLink *parent_link); 142 /// getNumLinks() const143 unsigned int getNumLinks() const { return m_link_list.size(); } 144 /// 145 dmLink *getLink(unsigned int index) const; 146 /// 147 int getLinkIndex(dmLink *link) const; 148 /// 149 dmLink *getLinkParent(unsigned int index) const; 150 151 /// getNumDOFs() const152 unsigned int getNumDOFs() const { return m_num_state_vars; } 153 154 /// 155 void setJointInput(Float joint_input[]); 156 /// 157 void setState(Float q[], Float qd[]); 158 /// 159 void getState(Float q[], Float qd[]) const; 160 161 /// 162 bool forwardKinematics(unsigned int link_index, 163 HomogeneousTransformationMatrix mat) const; 164 165 /// 166 virtual bool forwardKinematics(unsigned int link_index, 167 dmABForKinStruct &fk) const; 168 169 /// 170 const dmABForKinStruct *getForKinStruct(unsigned int link_index) const; 171 172 /// 173 virtual void pushForceStates(); 174 /// 175 virtual void popForceStates(); 176 /// 177 virtual Float getPotentialEnergy() const; 178 /// 179 virtual Float getKineticEnergy() const; 180 181 // dynamic algorithm 182 /// 183 void dynamics(Float *qy, Float *qdy); 184 185 // rendering function: 186 /// 187 void draw() const; 188 189 protected: 190 // not implemented 191 dmArticulation(const dmArticulation &); 192 dmArticulation &operator=(const dmArticulation &); 193 194 // AB algorithm functions: 195 virtual void ABForwardKinematics(Float q[], Float qd[], 196 const dmABForKinStruct &ref_val); 197 virtual void ABBackwardDynamics(); 198 virtual void ABForwardAccelerations(SpatialVector a_ref, 199 Float qd[], Float qdd[]); 200 201 struct LinkInfoStruct 202 { 203 unsigned int index; 204 dmLink *link; 205 206 LinkInfoStruct *parent; 207 vector<LinkInfoStruct*> child_list; 208 209 dmABForKinStruct link_val; 210 211 // AB algorithm temporaries 212 SpatialVector accel; 213 SpatialVector f_star; 214 SpatialTensor I_refl; 215 }; 216 217 void drawTraversal(LinkInfoStruct *node) const; 218 219 protected: 220 vector<LinkInfoStruct*> m_link_list; 221 unsigned int m_num_state_vars; 222 223 private: 224 // preallocated ABDynamics() simulation temporary variables. 225 dmABForKinStruct m_ref_val; 226 SpatialTensor m_I_star_ref; 227 SpatialVector m_beta_star_ref; 228 SpatialVector m_accel_ref; 229 }; 230 231 #endif 232