Bullet Collision Detection & Physics Library
btMultiBodyJointMotor.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 
17 
18 #include "btMultiBodyJointMotor.h"
19 #include "btMultiBody.h"
22 
23 
24 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
25  :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
26  m_desiredVelocity(desiredVelocity),
27  m_desiredPosition(0),
28  m_kd(1.),
29  m_kp(0),
30  m_erp(1),
31  m_rhsClamp(SIMD_INFINITY)
32 {
33 
34  m_maxAppliedImpulse = maxMotorImpulse;
35  // the data.m_jacobians never change, so may as well
36  // initialize them here
37 
38 
39 }
40 
42 {
44  // note: we rely on the fact that data.m_jacobians are
45  // always initialized to zero by the Constraint ctor
46  int linkDoF = 0;
47  unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
48 
49  // row 0: the lower bound
50  // row 0: the lower bound
51  jacobianA(0)[offset] = 1;
52 
54 }
55 
56 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
57  //:btMultiBodyConstraint(body,0,link,-1,1,true),
58  :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
59  m_desiredVelocity(desiredVelocity),
61  m_kd(1.),
62  m_kp(0),
63  m_erp(1),
65 {
66  btAssert(linkDoF < body->getLink(link).m_dofCount);
67 
68  m_maxAppliedImpulse = maxMotorImpulse;
69 
70 }
72 {
73 }
74 
76 {
78  if (col)
79  return col->getIslandTag();
80  for (int i=0;i<m_bodyA->getNumLinks();i++)
81  {
82  if (m_bodyA->getLink(i).m_collider)
83  return m_bodyA->getLink(i).m_collider->getIslandTag();
84  }
85  return -1;
86 }
87 
89 {
91  if (col)
92  return col->getIslandTag();
93 
94  for (int i=0;i<m_bodyB->getNumLinks();i++)
95  {
96  col = m_bodyB->getLink(i).m_collider;
97  if (col)
98  return col->getIslandTag();
99  }
100  return -1;
101 }
102 
103 
106  const btContactSolverInfo& infoGlobal)
107 {
108  // only positions need to be updated -- data.m_jacobians and force
109  // directions were set in the ctor and never change.
110 
112  {
114  }
115 
116  //don't crash
118  return;
119 
120  if (m_maxAppliedImpulse==0.f)
121  return;
122 
123  const btScalar posError = 0;
124  const btVector3 dummy(0, 0, 0);
125 
126  for (int row=0;row<getNumRows();row++)
127  {
128  btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
129 
130  int dof = 0;
131  btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
132  btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
133  btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
134 
135  btScalar velocityError = (m_desiredVelocity - currentVelocity);
136  btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
137  if (rhs>m_rhsClamp)
138  {
139  rhs=m_rhsClamp;
140  }
141  if (rhs<-m_rhsClamp)
142  {
143  rhs=-m_rhsClamp;
144  }
145 
146 
147  fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,rhs);
148  constraintRow.m_orgConstraint = this;
149  constraintRow.m_orgDofIndex = row;
150  {
151  //expect either prismatic or revolute joint type for now
154  {
156  {
157  constraintRow.m_contactNormal1.setZero();
158  constraintRow.m_contactNormal2.setZero();
160  constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
161  constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
162 
163  break;
164  }
166  {
168  constraintRow.m_contactNormal1=prismaticAxisInWorld;
169  constraintRow.m_contactNormal2=-prismaticAxisInWorld;
170  constraintRow.m_relpos1CrossNormal.setZero();
171  constraintRow.m_relpos2CrossNormal.setZero();
172 
173  break;
174  }
175  default:
176  {
177  btAssert(0);
178  }
179  };
180 
181  }
182 
183  }
184 
185 }
186 
btScalar * jacobianB(int row)
int getNumLinks() const
Definition: btMultiBody.h:164
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
btMultiBodyConstraint * m_orgConstraint
#define btAssert(x)
Definition: btScalar.h:131
btScalar * jacobianA(int row)
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btScalar * getJointVelMultiDof(int i)
virtual int getIslandIdA() const
btMultiBodyJointMotor(btMultiBody *body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
This file was written by Erwin Coumans.
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:917
#define SIMD_INFINITY
Definition: btScalar.h:522
void setZero()
Definition: btVector3.h:683
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
int getIslandTag() const
virtual int getIslandIdB() const
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btVector3 &constraintNormalAng, const btVector3 &constraintNormalLin, const btVector3 &posAworld, const btVector3 &posBworld, btScalar posError, const btContactSolverInfo &infoGlobal, btScalar lowerLimit, btScalar upperLimit, bool angConstraint=false, btScalar relaxation=1.f, bool isFriction=false, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btScalar * getJointPosMultiDof(int i)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292