Bullet Collision Detection & Physics Library
btMultiBodyJointMotor.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
17
19#include "btMultiBody.h"
22
23
24btMultiBodyJointMotor::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
56btMultiBodyJointMotor::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),
60 m_desiredPosition(0),
61 m_kd(1.),
62 m_kp(0),
63 m_erp(1),
64 m_rhsClamp(SIMD_INFINITY)
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 {
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
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:917
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define SIMD_INFINITY
Definition: btScalar.h:522
#define btAssert(x)
Definition: btScalar.h:131
int getIslandTag() const
btScalar * jacobianA(int row)
btScalar * jacobianB(int row)
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)
virtual int getIslandIdB() const
btMultiBodyJointMotor(btMultiBody *body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
This file was written by Erwin Coumans.
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
virtual int getIslandIdA() const
btScalar * getJointPosMultiDof(int i)
int getNumLinks() const
Definition: btMultiBody.h:164
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
btScalar * getJointVelMultiDof(int i)
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
void setZero()
Definition: btVector3.h:683
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btMultiBodyConstraint * m_orgConstraint