1 #ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_CYLINDER_H_ 2 #define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_CYLINDER_H_ 3 4 /* -------------------------------------------------------------------------- * 5 * Simbody(tm) * 6 * -------------------------------------------------------------------------- * 7 * This is part of the SimTK biosimulation toolkit originating from * 8 * Simbios, the NIH National Center for Physics-Based Simulation of * 9 * Biological Structures at Stanford, funded under the NIH Roadmap for * 10 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. * 11 * * 12 * Portions copyright (c) 2005-12 Stanford University and the Authors. * 13 * Authors: Michael Sherman * 14 * Contributors: Derived from IVM code written by Charles Schwieters * 15 * * 16 * Licensed under the Apache License, Version 2.0 (the "License"); you may * 17 * not use this file except in compliance with the License. You may obtain a * 18 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * 19 * * 20 * Unless required by applicable law or agreed to in writing, software * 21 * distributed under the License is distributed on an "AS IS" BASIS, * 22 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * 23 * See the License for the specific language governing permissions and * 24 * limitations under the License. * 25 * -------------------------------------------------------------------------- */ 26 27 28 /**@file 29 * Define the RigidBodyNode that implements a Cylinder mobilizer. 30 */ 31 32 #include "SimbodyMatterSubsystemRep.h" 33 #include "RigidBodyNode.h" 34 #include "RigidBodyNodeSpec.h" 35 36 37 // CYLINDER // 38 39 // This is a "cylinder" joint, meaning one degree of rotational freedom 40 // about a particular axis, and one degree of translational freedom 41 // along the same axis. For molecules you can think of this as a combination 42 // of torsion and bond stretch. The axis used is the z axis of the parent's 43 // F frame, which is aligned forever with the z axis of the body's M frame. 44 // In addition, the origin points of M and F are separated only along the 45 // z axis; i.e., they have the same x & y coords in the F frame. The two 46 // generalized coordinates are the rotation and the translation, in that order. 47 template<bool noX_MB, bool noR_PF> 48 class RBNodeCylinder : public RigidBodyNodeSpec<2, false, noX_MB, noR_PF> { 49 public: 50 typedef typename RigidBodyNodeSpec<2, false, noX_MB, noR_PF>::HType HType; type()51 virtual const char* type() { return "cylinder"; } 52 RBNodeCylinder(const MassProperties & mProps_B,const Transform & X_PF,const Transform & X_BM,bool isReversed,UIndex & nextUSlot,USquaredIndex & nextUSqSlot,QIndex & nextQSlot)53 RBNodeCylinder(const MassProperties& mProps_B, 54 const Transform& X_PF, 55 const Transform& X_BM, 56 bool isReversed, 57 UIndex& nextUSlot, 58 USquaredIndex& nextUSqSlot, 59 QIndex& nextQSlot) 60 : RigidBodyNodeSpec<2, false, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot, 61 RigidBodyNode::QDotIsAlwaysTheSameAsU, RigidBodyNode::QuaternionIsNeverUsed, isReversed) 62 { 63 this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot); 64 } 65 66 setQToFitRotationImpl(const SBStateDigest & sbs,const Rotation & R_FM,Vector & q)67 void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM, Vector& q) const { 68 // The only rotation our cylinder joint can handle is about z. 69 // TODO: this code is bad -- see comments for Torsion joint above. 70 const Vec3 angles123 = R_FM.convertRotationToBodyFixedXYZ(); 71 this->toQ(q)[0] = angles123[2]; 72 } 73 setQToFitTranslationImpl(const SBStateDigest & sbs,const Vec3 & p_FM,Vector & q)74 void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM, Vector& q) const { 75 // Because the M and F origins must lie along their shared z axis, there is no way to 76 // create a translation by rotating around z. So the only translation we can represent 77 // is that component which is along z. 78 this->toQ(q)[1] = p_FM[2]; 79 } 80 setUToFitAngularVelocityImpl(const SBStateDigest & sbs,const Vector &,const Vec3 & w_FM,Vector & u)81 void setUToFitAngularVelocityImpl(const SBStateDigest& sbs, const Vector&, const Vec3& w_FM, Vector& u) const { 82 // We can only represent an angular velocity along z with this joint. 83 this->toU(u)[0] = w_FM[2]; 84 } 85 setUToFitLinearVelocityImpl(const SBStateDigest & sbs,const Vector &,const Vec3 & v_FM,Vector & u)86 void setUToFitLinearVelocityImpl 87 (const SBStateDigest& sbs, const Vector&, const Vec3& v_FM, Vector& u) const 88 { 89 // Because the M and F origins must lie along their shared z axis, there is no way to 90 // create a linear velocity by rotating around z. So the only linear velocity we can represent 91 // is that component which is along z. 92 this->toU(u)[1] = v_FM[2]; 93 } 94 95 enum {CosQ=0, SinQ=1}; 96 // We want space for cos(q0) and sin(q0). calcQPoolSize(const SBModelVars &)97 int calcQPoolSize(const SBModelVars&) const 98 { return 2; } 99 performQPrecalculations(const SBStateDigest & sbs,const Real * q,int nq,Real * qCache,int nQCache,Real * qErr,int nQErr)100 void performQPrecalculations(const SBStateDigest& sbs, 101 const Real* q, int nq, 102 Real* qCache, int nQCache, 103 Real* qErr, int nQErr) const 104 { 105 assert(q && nq==2 && qCache && nQCache==2 && nQErr==0); 106 qCache[CosQ] = std::cos(q[0]); 107 qCache[SinQ] = std::sin(q[0]); 108 } 109 calcX_FM(const SBStateDigest & sbs,const Real * q,int nq,const Real * qCache,int nQCache,Transform & X_FM)110 void calcX_FM(const SBStateDigest& sbs, 111 const Real* q, int nq, 112 const Real* qCache, int nQCache, 113 Transform& X_FM) const 114 { 115 assert(q && nq==2 && qCache && nQCache==2); 116 X_FM.updR().setRotationFromAngleAboutZ(qCache[CosQ], qCache[SinQ]); 117 X_FM.updP() = Vec3(0,0,q[1]); 118 } 119 120 121 // The generalized speeds are (1) the angular velocity of M in the F frame, 122 // about F's z axis, expressed in F, and (2) the velocity of M's origin 123 // in F, along F's z axis. (The z axis is also constant in M for this joint.) calcAcrossJointVelocityJacobian(const SBStateDigest & sbs,HType & H_FM)124 void calcAcrossJointVelocityJacobian( 125 const SBStateDigest& sbs, 126 HType& H_FM) const 127 { 128 H_FM(0) = SpatialVec( Vec3(0,0,1), Vec3(0) ); 129 H_FM(1) = SpatialVec( Vec3(0), Vec3(0,0,1) ); 130 } 131 132 // Since the Jacobian above is constant in F, its time derivative in F is zero. calcAcrossJointVelocityJacobianDot(const SBStateDigest & sbs,HType & HDot_FM)133 void calcAcrossJointVelocityJacobianDot( 134 const SBStateDigest& sbs, 135 HType& HDot_FM) const 136 { 137 HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) ); 138 HDot_FM(1) = SpatialVec( Vec3(0), Vec3(0) ); 139 } 140 141 // Override the computation of reverse-H for this simple mobilizer. calcReverseMobilizerH_FM(const SBStateDigest & sbs,HType & H_FM)142 void calcReverseMobilizerH_FM( 143 const SBStateDigest& sbs, 144 HType& H_FM) const 145 { 146 H_FM(0) = SpatialVec( Vec3(0,0,-1), Vec3(0) ); 147 H_FM(1) = SpatialVec( Vec3(0), Vec3(0,0,-1) ); 148 } 149 150 // Override the computation of reverse-HDot for this simple mobilizer. calcReverseMobilizerHDot_FM(const SBStateDigest & sbs,HType & HDot_FM)151 void calcReverseMobilizerHDot_FM( 152 const SBStateDigest& sbs, 153 HType& HDot_FM) const 154 { 155 HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) ); 156 HDot_FM(1) = SpatialVec( Vec3(0), Vec3(0) ); 157 } 158 }; 159 160 161 162 #endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_CYLINDER_H_ 163 164