1 /* Siconos is a program dedicated to modeling, simulation and control 2 * of non smooth dynamical systems. 3 * 4 * Copyright 2021 INRIA. 5 * 6 * Licensed under the Apache License, Version 2.0 (the "License"); 7 * you may not use this file except in compliance with the License. 8 * You may obtain a copy of the License at 9 * 10 * http://www.apache.org/licenses/LICENSE-2.0 11 * 12 * Unless required by applicable law or agreed to in writing, software 13 * distributed under the License is distributed on an "AS IS" BASIS, 14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 * See the License for the specific language governing permissions and 16 * limitations under the License. 17 */ 18 /*! \file CouplerJointR.hpp 19 */ 20 #ifndef CouplerJointRELATION_H 21 #define CouplerJointRELATION_H 22 23 #include <MechanicsFwd.hpp> 24 #include <SiconosFwd.hpp> 25 #include <NewtonEulerR.hpp> 26 #include <NewtonEulerJointR.hpp> 27 #include <Tools.hpp> 28 29 /** \class CouplerJointR 30 * \brief This class implements a coupling (equality) between two 31 * DoFs of any NewtonEulerJointR. Can be used e.g. to implement a 32 * screw relation (coupled rotation and translation) based on 33 * CylindricalJointR. 34 */ 35 class CouplerJointR : public NewtonEulerJointR 36 { 37 protected: 38 /** serialization hooks 39 */ 40 ACCEPT_SERIALIZATION(CouplerJointR); 41 42 SP::NewtonEulerJointR _joint1, _joint2; 43 SP::SiconosVector _ref1, _ref2; 44 45 unsigned int _dof1, _dof2; 46 unsigned int _ref1_index, _ref2_index; 47 double _ratio, _offset; 48 49 /** Return the normal of the linear DoF axis. \param axis must be 0 */ 50 virtual void _normalDoF(SiconosVector& ans, const BlockVector& q0, int axis, 51 bool absoluteRef=true); 52 53 /** An internal helper function to assign reference vectors during 54 * computeh and computeJachq. */ 55 void makeBlockVectors(SP::SiconosVector q1, SP::SiconosVector q2, 56 BlockVector& q01, BlockVector& q02); 57 58 public: 59 60 /** Empty constructor. The relation may be initialized later by 61 * setPoint, setAbsolute, and setBasePositions. */ 62 CouplerJointR(); 63 64 /** Initialize a coupler. See setReferences() for an explanation of 65 * the parameters. */ 66 CouplerJointR(SP::NewtonEulerJointR joint1, unsigned int dof1, 67 SP::NewtonEulerJointR joint2, unsigned int dof2, 68 double ratio, 69 SP::SiconosVector ref1=SP::SiconosVector(), unsigned int ref1_index=0, 70 SP::SiconosVector ref2=SP::SiconosVector(), unsigned int ref2_index=0); 71 72 /** Initialize a coupler. See setReferences() for an explanation of 73 * the parameters. */ 74 CouplerJointR(SP::NewtonEulerJointR joint1, unsigned int dof1, 75 SP::NewtonEulerJointR joint2, unsigned int dof2, 76 double ratio, 77 SP::NewtonEulerDS refds1, unsigned int ref1_index, 78 SP::NewtonEulerDS refds2, unsigned int ref2_index); 79 80 /** to compute the output y = h(t,q,z) of the Relation 81 \param time current time value 82 \param q coordinates of the dynamical systems involved in the relation 83 \param y the resulting vector 84 */ 85 virtual void computeh(double time, const BlockVector& q0, SiconosVector& y); 86 87 virtual void computeJachq(double time, Interaction& inter, SP::BlockVector q0); 88 89 /* Return the joint DoF index assigned to the first DoF. */ dof1()90 unsigned int dof1() { return _dof1; } 91 92 /* Return the joint DoF index assigned to the second DoF. */ dof2()93 unsigned int dof2() { return _dof2; } 94 95 /* Return the first reference joint assigned to this friction relation. */ joint1()96 SP::NewtonEulerJointR joint1() { return _joint1; } 97 98 /* Return the second reference joint assigned to this friction relation. */ joint2()99 SP::NewtonEulerJointR joint2() { return _joint2; } 100 101 /** Set reference joints and vectors. This constraint maintains the 102 * equality theta2=theta1*ratio; theta1 is measured by joint1 with 103 * reference to some vector ref1 which must replace either the 104 * first or second DS of the current relation being defined. If 105 * ref1 and ref2 are null, then no reference is used. Typically 106 * ref1 and ref2 will be equal in order to implement gear ratios 107 * for example. joint1 must be between ref1 and ds1 specified in 108 * setBasePositions(), while joint2 must be between ref2 and ds2. 109 * 110 * \param joint1 The joint for the first reference measurement theta1. 111 * \param dof1 The degree of freedom index of joint1 to use for 112 * the first reference measurement theta1. 113 * \param ref1 The optional reference position for the first 114 * reference measurement theta1. 115 * \param ref1_index Must be 0 or 1, depending on where ref1 116 * appears in joint1. 117 * \param joint2 The joint for the second reference measurement theta2. 118 * \param dof2 The degree of freedom index of joint2 to use for 119 * the second reference measurement theta2. 120 * \param ref2 The optional reference position for the second 121 * reference measurement theta2. 122 * \param ref2_index Must be 0 or 1, depending on where ref2 123 * appears in joint2. 124 */ 125 void setReferences(SP::NewtonEulerJointR joint1, unsigned int dof1, 126 SP::NewtonEulerJointR joint2, unsigned int dof2, 127 SP::SiconosVector ref1, unsigned int ref1_index, 128 SP::SiconosVector ref2, unsigned int ref2_index); 129 130 /** Set reference joints and vectors. This constraint maintains the 131 * equality theta2=theta1*ratio; theta1 is measured by joint1 with 132 * reference to some vector ref1 which must replace either the 133 * first or second DS of the current relation being defined. If 134 * ref1 and ref2 are null, then no reference is used. Typically 135 * ref1 and ref2 will be equal in order to implement gear ratios 136 * for example. joint1 must be between ref1 and ds1 specified in 137 * setBasePositions(), while joint2 must be between ref2 and ds2. 138 * This alternative setReferences() takes NewtonEulerDS as 139 * parameters, but the reference vectors will be taken as 140 * refds1->q() and refds2->q() respectively. 141 * 142 * \param joint1 The joint for the first reference measurement theta1. 143 * \param dof1 The degree of freedom index of joint1 to use for 144 * the first reference measurement theta1. 145 * \param refds1 The optional reference vector for the first 146 * reference measurement theta1. 147 * \param ref1_index Must be 0 or 1, depending on where ref1 148 * appears in joint1. 149 * \param joint2 The joint for the second reference measurement theta2. 150 * \param dof2 The degree of freedom index of joint2 to use for 151 * the second reference measurement theta2. 152 * \param refds2 The optional reference vector for the second 153 * reference measurement theta2. 154 * \param ref2_index Must be 0 or 1, depending on where ref2 155 * appears in joint2. 156 */ 157 void setReferences(SP::NewtonEulerJointR joint1, unsigned int dof1, 158 SP::NewtonEulerJointR joint2, unsigned int dof2, 159 SP::NewtonEulerDS refds1, unsigned int ref1_index, 160 SP::NewtonEulerDS refds2, unsigned int ref2_index); 161 162 /* Set the gear ratio. */ 163 void setRatio(double ratio); 164 165 /* From provided position vectors, calculate initial offsets to be 166 * maintained in the relation. */ 167 void setBasePositions(SP::SiconosVector q1, 168 SP::SiconosVector q2=SP::SiconosVector()); 169 170 /** Get the number of constraints defined in the joint 171 \return the number of constraints 172 */ numberOfConstraints()173 virtual unsigned int numberOfConstraints() { return 1; } 174 175 /** Get the number of degrees of freedom defined in the joint 176 \return the number of degrees of freedom (DoF) 177 */ numberOfDoF()178 virtual unsigned int numberOfDoF() { return 1; } 179 180 /** Return the type of a degree of freedom of this joint. 181 \return the type of the degree of freedom (DoF) 182 */ typeOfDoF(unsigned int axis)183 virtual DoF_Type typeOfDoF(unsigned int axis) { 184 if (axis<1) return DOF_TYPE_LINEAR; 185 else return DOF_TYPE_INVALID; 186 }; 187 188 /** Compute the vector of linear and angular positions of the free axes */ 189 virtual void computehDoF(double time, const BlockVector& q0, SiconosVector& y, 190 unsigned int axis); 191 192 /** Compute the jacobian of linear and angular DoF with respect to some q */ 193 virtual void computeJachqDoF(double time, Interaction& inter, 194 SP::BlockVector q0, SimpleMatrix& jachq, 195 unsigned int axis); 196 197 /** destructor 198 */ ~CouplerJointR()199 virtual ~CouplerJointR() {}; 200 }; 201 #endif //CouplerJointRELATION_H 202