1 #include "MyMultiBodyCreator.h"
2
3 #include "../../CommonInterfaces/CommonGUIHelperInterface.h"
4
5 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
6 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
7
8 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
9
10 #include "btBulletDynamicsCommon.h"
11 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
12 #include "URDFJointTypes.h"
13
MyMultiBodyCreator(GUIHelperInterface * guiHelper)14 MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
15 : m_bulletMultiBody(0),
16 m_rigidBody(0),
17 m_guiHelper(guiHelper)
18 {
19 }
20
allocateMultiBody(int,int totalNumJoints,btScalar mass,const btVector3 & localInertiaDiagonal,bool isFixedBase,bool canSleep)21 class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints, btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep)
22 {
23 // m_urdf2mbLink.resize(totalNumJoints+1,-2);
24 m_mb2urdfLink.resize(totalNumJoints + 1, -2);
25
26 m_bulletMultiBody = new btMultiBody(totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
27 //if (canSleep)
28 // m_bulletMultiBody->goToSleep();
29 return m_bulletMultiBody;
30 }
31
allocateRigidBody(int urdfLinkIndex,btScalar mass,const btVector3 & localInertiaDiagonal,const btTransform & initialWorldTrans,class btCollisionShape * colShape)32 class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape)
33 {
34 btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
35 rbci.m_startWorldTransform = initialWorldTrans;
36 btScalar sleep_threshold = btScalar(0.22360679775);//sqrt(0.05) to be similar to btMultiBody (SLEEP_THRESHOLD)
37 rbci.m_angularSleepingThreshold = sleep_threshold;
38 rbci.m_linearSleepingThreshold = sleep_threshold;
39
40 btRigidBody* body = new btRigidBody(rbci);
41 if (m_rigidBody == 0)
42 {
43 //only store the root of the multi body
44 m_rigidBody = body;
45 }
46 return body;
47 }
48
allocateMultiBodyLinkCollider(int,int mbLinkIndex,btMultiBody * multiBody)49 class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)
50 {
51 btMultiBodyLinkCollider* mbCol = new btMultiBodyLinkCollider(multiBody, mbLinkIndex);
52 return mbCol;
53 }
54
allocateGeneric6DofSpring2Constraint(int urdfLinkIndex,btRigidBody & rbA,btRigidBody & rbB,const btTransform & offsetInA,const btTransform & offsetInB,int rotateOrder)55 class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder)
56 {
57 btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA, rbB, offsetInA, offsetInB, (RotateOrder)rotateOrder);
58
59 return c;
60 }
61
createPrismaticJoint(int urdfLinkIndex,btRigidBody & rbA,btRigidBody & rbB,const btTransform & offsetInA,const btTransform & offsetInB,const btVector3 & jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit)62 class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
63 const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit)
64 {
65 int rotateOrder = 0;
66 btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, rotateOrder);
67 //todo(erwincoumans) for now, we only support principle axis along X, Y or Z
68 int principleAxis = jointAxisInJointSpace.closestAxis();
69
70 GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
71 userInfo->m_jointAxisInJointSpace = jointAxisInJointSpace;
72 userInfo->m_jointAxisIndex = principleAxis;
73
74 userInfo->m_urdfJointType = URDFPrismaticJoint;
75 userInfo->m_lowerJointLimit = jointLowerLimit;
76 userInfo->m_upperJointLimit = jointUpperLimit;
77 userInfo->m_urdfIndex = urdfLinkIndex;
78 dof6->setUserConstraintPtr(userInfo);
79
80 switch (principleAxis)
81 {
82 case 0:
83 {
84 dof6->setLinearLowerLimit(btVector3(jointLowerLimit, 0, 0));
85 dof6->setLinearUpperLimit(btVector3(jointUpperLimit, 0, 0));
86 break;
87 }
88 case 1:
89 {
90 dof6->setLinearLowerLimit(btVector3(0, jointLowerLimit, 0));
91 dof6->setLinearUpperLimit(btVector3(0, jointUpperLimit, 0));
92 break;
93 }
94 case 2:
95 default:
96 {
97 dof6->setLinearLowerLimit(btVector3(0, 0, jointLowerLimit));
98 dof6->setLinearUpperLimit(btVector3(0, 0, jointUpperLimit));
99 }
100 };
101
102 dof6->setAngularLowerLimit(btVector3(0, 0, 0));
103 dof6->setAngularUpperLimit(btVector3(0, 0, 0));
104 m_6DofConstraints.push_back(dof6);
105 return dof6;
106 }
107
createRevoluteJoint(int urdfLinkIndex,btRigidBody & rbA,btRigidBody & rbB,const btTransform & offsetInA,const btTransform & offsetInB,const btVector3 & jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit)108 class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
109 const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit)
110 {
111 btGeneric6DofSpring2Constraint* dof6 = 0;
112
113 //only handle principle axis at the moment,
114 //@todo(erwincoumans) orient the constraint for non-principal axis
115 int principleAxis = jointAxisInJointSpace.closestAxis();
116 switch (principleAxis)
117 {
118 case 0:
119 {
120 dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, RO_ZYX);
121 dof6->setLinearLowerLimit(btVector3(0, 0, 0));
122 dof6->setLinearUpperLimit(btVector3(0, 0, 0));
123
124 dof6->setAngularLowerLimit(btVector3(jointLowerLimit, 0, 0));
125 dof6->setAngularUpperLimit(btVector3(jointUpperLimit, 0, 0));
126
127 break;
128 }
129 case 1:
130 {
131 dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, RO_XZY);
132 dof6->setLinearLowerLimit(btVector3(0, 0, 0));
133 dof6->setLinearUpperLimit(btVector3(0, 0, 0));
134
135 dof6->setAngularLowerLimit(btVector3(0, jointLowerLimit, 0));
136 dof6->setAngularUpperLimit(btVector3(0, jointUpperLimit, 0));
137 break;
138 }
139 case 2:
140 default:
141 {
142 dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, RO_XYZ);
143 dof6->setLinearLowerLimit(btVector3(0, 0, 0));
144 dof6->setLinearUpperLimit(btVector3(0, 0, 0));
145
146 dof6->setAngularLowerLimit(btVector3(0, 0, jointLowerLimit));
147 dof6->setAngularUpperLimit(btVector3(0, 0, jointUpperLimit));
148 }
149 };
150
151 GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
152 userInfo->m_jointAxisInJointSpace = jointAxisInJointSpace;
153 userInfo->m_jointAxisIndex = 3 + principleAxis;
154
155 if (jointLowerLimit > jointUpperLimit)
156 {
157 userInfo->m_urdfJointType = URDFContinuousJoint;
158 }
159 else
160 {
161 userInfo->m_urdfJointType = URDFRevoluteJoint;
162 userInfo->m_lowerJointLimit = jointLowerLimit;
163 userInfo->m_upperJointLimit = jointUpperLimit;
164 }
165 userInfo->m_urdfIndex = urdfLinkIndex;
166 dof6->setUserConstraintPtr(userInfo);
167 m_6DofConstraints.push_back(dof6);
168 return dof6;
169 }
170
createFixedJoint(int urdfLinkIndex,btRigidBody & rbA,btRigidBody & rbB,const btTransform & offsetInA,const btTransform & offsetInB)171 class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB)
172 {
173 btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB);
174
175 GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
176 userInfo->m_urdfIndex = urdfLinkIndex;
177 userInfo->m_urdfJointType = URDFFixedJoint;
178
179 dof6->setUserConstraintPtr(userInfo);
180
181 dof6->setLinearLowerLimit(btVector3(0, 0, 0));
182 dof6->setLinearUpperLimit(btVector3(0, 0, 0));
183
184 dof6->setAngularLowerLimit(btVector3(0, 0, 0));
185 dof6->setAngularUpperLimit(btVector3(0, 0, 0));
186 m_6DofConstraints.push_back(dof6);
187 return dof6;
188 }
189
addLinkMapping(int urdfLinkIndex,int mbLinkIndex)190 void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
191 {
192 if (m_mb2urdfLink.size() < (mbLinkIndex + 1))
193 {
194 m_mb2urdfLink.resize((mbLinkIndex + 1), -2);
195 }
196 // m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
197 m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
198 }
199
createRigidBodyGraphicsInstance(int linkIndex,btRigidBody * body,const btVector3 & colorRgba,int graphicsIndex)200 void MyMultiBodyCreator::createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex)
201 {
202 m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
203 }
204
createRigidBodyGraphicsInstance2(int linkIndex,class btRigidBody * body,const btVector3 & colorRgba,const btVector3 & specularColor,int graphicsIndex)205 void MyMultiBodyCreator::createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex)
206 {
207 m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
208 int graphicsInstanceId = body->getUserIndex();
209 btVector3DoubleData speculard;
210 specularColor.serializeDouble(speculard);
211 m_guiHelper->changeSpecularColor(graphicsInstanceId, speculard.m_floats);
212 }
213
createCollisionObjectGraphicsInstance(int linkIndex,class btCollisionObject * colObj,const btVector3 & colorRgba)214 void MyMultiBodyCreator::createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba)
215 {
216 m_guiHelper->createCollisionObjectGraphicsObject(colObj, colorRgba);
217 }
218
createCollisionObjectGraphicsInstance2(int linkIndex,class btCollisionObject * col,const btVector4 & colorRgba,const btVector3 & specularColor)219 void MyMultiBodyCreator::createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor)
220 {
221 createCollisionObjectGraphicsInstance(linkIndex, col, colorRgba);
222 int graphicsInstanceId = col->getUserIndex();
223 btVector3DoubleData speculard;
224 specularColor.serializeDouble(speculard);
225 m_guiHelper->changeSpecularColor(graphicsInstanceId, speculard.m_floats);
226 }
227
getBulletMultiBody()228 btMultiBody* MyMultiBodyCreator::getBulletMultiBody()
229 {
230 return m_bulletMultiBody;
231 }
232