Bullet Collision Detection & Physics Library
btMultiBodySphericalJointMotor.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2018 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 
19 #include "btMultiBody.h"
24 
26  : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true),
27  m_desiredVelocity(0, 0, 0),
28  m_desiredPosition(0,0,0,1),
29  m_kd(1.),
30  m_kp(0.2),
31  m_erp(1),
32  m_rhsClamp(SIMD_INFINITY)
33 {
34 
35  m_maxAppliedImpulse = maxMotorImpulse;
36 }
37 
38 
40 {
42  // note: we rely on the fact that data.m_jacobians are
43  // always initialized to zero by the Constraint ctor
44  int linkDoF = 0;
45  unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
46 
47  // row 0: the lower bound
48  // row 0: the lower bound
49  jacobianA(0)[offset] = 1;
50 
52 }
53 
54 
56 {
57 }
58 
60 {
61  if (this->m_linkA < 0)
62  {
64  if (col)
65  return col->getIslandTag();
66  }
67  else
68  {
70  {
72  }
73  }
74  return -1;
75 }
76 
78 {
79  if (m_linkB < 0)
80  {
82  if (col)
83  return col->getIslandTag();
84  }
85  else
86  {
88  {
90  }
91  }
92  return -1;
93 }
94 
97  const btContactSolverInfo& infoGlobal)
98 {
99  // only positions need to be updated -- data.m_jacobians and force
100  // directions were set in the ctor and never change.
101 
103  {
105  }
106 
107  //don't crash
109  return;
110 
111 
112  if (m_maxAppliedImpulse == 0.f)
113  return;
114 
115  const btScalar posError = 0;
116  const btVector3 dummy(0, 0, 0);
117 
118 
119  btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
120 
121  btQuaternion desiredQuat = m_desiredPosition;
126 
127 btQuaternion relRot = currentQuat.inverse() * desiredQuat;
128  btVector3 angleDiff;
130 
131 
132 
133  for (int row = 0; row < getNumRows(); row++)
134  {
135  btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
136 
137  int dof = row;
138 
139  btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
140  btScalar desiredVelocity = this->m_desiredVelocity[row];
141 
142  btScalar velocityError = desiredVelocity - currentVelocity;
143 
144  btMatrix3x3 frameAworld;
145  frameAworld.setIdentity();
146  frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
147  btScalar posError = 0;
148  {
151  {
153  {
154  btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
155  posError = m_kp*angleDiff[row % 3];
156  fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
157  btVector3(0,0,0), dummy, dummy,
158  posError,
159  infoGlobal,
161  constraintRow.m_orgConstraint = this;
162  constraintRow.m_orgDofIndex = row;
163  break;
164  }
165  default:
166  {
167  btAssert(0);
168  }
169  };
170  }
171  }
172 }
btMultiBody::getJointPosMultiDof
btScalar * getJointPosMultiDof(int i)
Definition: btMultiBody.cpp:378
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:49
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btContactSolverInfo
Definition: btContactSolverInfo.h:69
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:497
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:40
btGeneric6DofSpring2Constraint.h
btMultiBody::getJointVelMultiDof
btScalar * getJointVelMultiDof(int i)
Definition: btMultiBody.cpp:383
btMultiBodySphericalJointMotor::m_kp
btScalar m_kp
Definition: btMultiBodySphericalJointMotor.h:30
btMultiBodySphericalJointMotor::m_desiredVelocity
btVector3 m_desiredVelocity
Definition: btMultiBodySphericalJointMotor.h:27
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:46
btMultiBodyConstraint::getNumRows
int getNumRows() const
Definition: btMultiBodyConstraint.h:104
btTransformUtil.h
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btAssert
#define btAssert(x)
Definition: btScalar.h:133
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:28
btMultiBody::localFrameToWorld
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const
Definition: btMultiBody.cpp:534
btMultiBodySphericalJointMotor::m_desiredPosition
btQuaternion m_desiredPosition
Definition: btMultiBodySphericalJointMotor.h:28
btMultiBodySphericalJointMotor::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodySphericalJointMotor.cpp:95
btMultiBodyLinkCollider.h
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:47
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition: btMultiBodyConstraint.h:56
btMultiBodyConstraint::fillMultiBodyConstraint
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)
Definition: btMultiBodyConstraint.cpp:52
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:32
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:133
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:524
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btGeneric6DofSpring2Constraint::matrixToEulerXYZ
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:68
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btMultiBody
Definition: btMultiBody.h:49
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:434
btMultiBodyConstraint::m_numDofsFinalized
int m_numDofsFinalized
Definition: btMultiBodyConstraint.h:55
btAlignedObjectArray< btMultiBodySolverConstraint >
btMultiBodyConstraint::allocateJacobiansMultiDof
void allocateJacobiansMultiDof()
Definition: btMultiBodyConstraint.cpp:34
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition: btMultiBodyConstraint.h:160
btMultiBodySphericalJointMotor::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodySphericalJointMotor.cpp:77
btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor
virtual ~btMultiBodySphericalJointMotor()
Definition: btMultiBodySphericalJointMotor.cpp:55
btMultiBody.h
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:44
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btMultiBodySphericalJointMotor.h
btMatrix3x3::setIdentity
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:314
btCollisionObject.h
btMultiBodyConstraint::m_jacSizeBoth
int m_jacSizeBoth
Definition: btMultiBodyConstraint.h:51
btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor
btMultiBodySphericalJointMotor(btMultiBody *body, int link, btScalar maxMotorImpulse)
This file was written by Erwin Coumans.
Definition: btMultiBodySphericalJointMotor.cpp:25
btMultiBodySphericalJointMotor::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodySphericalJointMotor.cpp:59
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:237
btMultiBodySphericalJointMotor::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodySphericalJointMotor.cpp:39
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:45