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: dmStaticRootLink.hpp
21  *   Author: Duane Marhefka
22  *  Created: 1999
23  *  Summary:
24  *****************************************************************************/
25 
26 #ifndef _DM_STATIC_ROOT_LINK_HPP
27 #define _DM_STATIC_ROOT_LINK_HPP
28 
29 #include <dm.h>
30 #include <dmLink.hpp>
31 
32 /**
33 The {\tt dmStaticRootLink} class is derived from {\tt dmLink}.  It is a
34 massless link, whose only purpose is for use in systems containing kinematic
35 loops closed through a fixed base.  In such a case, a {\tt dmStaticRootLink}
36 object must be used as the root link of the entire system.  This satisfies a
37 criteria of the simulation algorithm that every loop has a root link.
38 
39 The static root link has no parameters and no degrees of freedom.  Its
40 coordinate system is coincident with that of its predecessor, which is generally
41 the system itself, as this link serves no useful purpose other than as mentioned
42 above. */
43 
44 class DM_DLL_API dmStaticRootLink : public dmLink
45 {
46    enum {NUM_DOFS = 0};
47 
48 public:
49    ///
dmStaticRootLink()50    dmStaticRootLink() {};
51    ///
~dmStaticRootLink()52    virtual ~dmStaticRootLink() {};
53 
54    ///
getNumDOFs() const55    inline int getNumDOFs() const {return NUM_DOFS;}
56    ///
setState(Float[],Float[])57    inline void setState(Float [], Float []) {};
58    ///
getState(Float[],Float[]) const59    inline void getState(Float [], Float []) const {};
60    ///
61    void getPose(RotationMatrix R, CartesianVector p) const;
62 
63    ///
setJointInput(Float[])64    inline void setJointInput(Float []) {};
65 
66    // link-to-link transformation functions:
67    ///
68    void   rtxToInboard(const CartesianVector p_0,
69                        CartesianVector p_inboard) const;
70    ///
71    void rtxFromInboard(const CartesianVector p_inboard,
72                        CartesianVector p_0) const;
73 
74    ///
75    void   stxToInboard(const SpatialVector p_0,
76                        SpatialVector p_inboard) const;
77    ///
78    void stxFromInboard(const SpatialVector p_inboard,
79                        SpatialVector p_0) const;
80 
81    ///
82    void rcongtxToInboardSym(const CartesianTensor M_0,
83                             CartesianTensor M_inboard) const;
84    ///
85    void rcongtxToInboardGen(const CartesianTensor M_0,
86                             CartesianTensor M_inboard) const;
87    ///
88    void scongtxToInboardIrefl(const SpatialTensor M_0,
89                               SpatialTensor M_inboard) const;
90    ///
91    void XikToInboard(Float **Xik_curr,
92                      Float **Xik_prev,
93                      int columns_Xik) const;
94    ///
95    void BToInboard(Float **Bkn,
96                    Float **Xik, int cols_Xik,
97                    Float **Xin, int cols_Xin) const;
98    ///
99    void xformZetak(Float *zetak,
100                    Float **Xik, int cols_Xik) const;
101 
102 // AB algorithm functions:
103    ///
104    void ABForwardKinematics(Float [],
105                             Float [],
106                             const dmABForKinStruct &link_val_inboard,
107                             dmABForKinStruct &link_val_curr);
108 
109    ///
110    void ABBackwardDynamics(const dmABForKinStruct &link_val_curr,
111                            SpatialVector f_star_curr,
112                            SpatialTensor I_refl_curr,
113                            SpatialVector f_star_inboard,
114                            SpatialTensor I_refl_inboard);
115    ///
116    void ABBackwardDynamicsN(const dmABForKinStruct &link_val_curr,
117                             SpatialVector f_star_inboard,
118                             SpatialTensor I_refl_inboard);
119 
120    ///
121    void ABForwardAccelerations(SpatialVector a_inboard,
122                                SpatialVector a_curr,
123                                Float [],
124                                Float []);
125    ///
126    void ABForwardAccelerations(SpatialVector a_inboard,
127                                unsigned int *LB,
128                                unsigned int num_elements_LB,
129                                Float ***Xik,
130                                Float **constraint_forces,
131                                unsigned int *num_constraints,
132                                SpatialVector a_curr,
133                                Float qd[],
134                                Float qdd[]);
135    ///
getPotentialEnergy(const dmABForKinStruct &,CartesianVector) const136    virtual Float getPotentialEnergy(const dmABForKinStruct &,
137                                     CartesianVector) const { return 0; }
138    ///
getKineticEnergy(const dmABForKinStruct &) const139    virtual Float getKineticEnergy(const dmABForKinStruct &) const { return 0; }
140 
141 // rendering function:
142    ///
143    virtual void draw() const;
144 
145 private:
146    dmStaticRootLink(const dmStaticRootLink &);
147    dmStaticRootLink &operator=(const dmStaticRootLink &);
148 };
149 
150 #endif
151