Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

move body handles in its own template class, for re-use.

Browse Source
This commit is contained in:
Erwin Coumans
2017-05-03 10:49:04 -07:00
parent 1bb133a01f
commit 2c8f65a2d0
2 changed files with 190 additions and 167 deletions
Download Patch File Download Diff File Expand all files Collapse all files

227
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -27,6 +27,8 @@
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "SharedMemoryCommands.h"
#include "LinearMath/btRandom.h"
#include "Bullet3Common/b3ResizablePool.h"
#ifdef B3_ENABLE_TINY_AUDIO
#include "../TinyAudio/b3SoundEngine.h"
#endif
@@ -156,21 +158,8 @@ struct InteralUserConstraintData
}
};
///todo: templatize this
struct InternalBodyHandle : public InteralBodyData
{
BT_DECLARE_ALIGNED_ALLOCATOR();
typedef b3PoolBodyHandle<InteralBodyData> InternalBodyHandle;
int m_nextFreeHandle;
void SetNextFree(int next)
{
m_nextFreeHandle = next;
}
int GetNextFree() const
{
return m_nextFreeHandle;
}
};
class btCommandChunk
{
@@ -1131,95 +1120,8 @@ struct ContactPointsStateLogger : public InternalStateLogger
struct PhysicsServerCommandProcessorInternalData
{
///handle management
btAlignedObjectArray<InternalBodyHandle> m_bodyHandles;
int m_numUsedHandles; // number of active handles
int m_firstFreeHandle; // free handles list
b3ResizablePool< InternalBodyHandle > m_bodyHandles;
int getNumHandles() const
{
return m_bodyHandles.size();
}
InternalBodyHandle* getHandle(int handle)
{
btAssert(handle>=0);
btAssert(handle<m_bodyHandles.size());
if ((handle<0) || (handle>=m_bodyHandles.size()))
{
return 0;
}
return &m_bodyHandles[handle];
}
const InternalBodyHandle* getHandle(int handle) const
{
return &m_bodyHandles[handle];
}
void increaseHandleCapacity(int extraCapacity)
{
int curCapacity = m_bodyHandles.size();
btAssert(curCapacity == m_numUsedHandles);
int newCapacity = curCapacity + extraCapacity;
m_bodyHandles.resize(newCapacity);
{
for (int i = curCapacity; i < newCapacity; i++)
m_bodyHandles[i].SetNextFree(i + 1);
m_bodyHandles[newCapacity - 1].SetNextFree(-1);
}
m_firstFreeHandle = curCapacity;
}
void initHandles()
{
m_numUsedHandles = 0;
m_firstFreeHandle = -1;
increaseHandleCapacity(1);
}
void exitHandles()
{
m_bodyHandles.resize(0);
m_firstFreeHandle = -1;
m_numUsedHandles = 0;
}
int allocHandle()
{
btAssert(m_firstFreeHandle>=0);
int handle = m_firstFreeHandle;
m_firstFreeHandle = getHandle(handle)->GetNextFree();
m_numUsedHandles++;
if (m_firstFreeHandle<0)
{
//int curCapacity = m_bodyHandles.size();
int additionalCapacity= m_bodyHandles.size();
increaseHandleCapacity(additionalCapacity);
getHandle(handle)->SetNextFree(m_firstFreeHandle);
}
return handle;
}
void freeHandle(int handle)
{
btAssert(handle >= 0);
getHandle(handle)->SetNextFree(m_firstFreeHandle);
m_firstFreeHandle = handle;
m_numUsedHandles--;
}
///end handle management
bool m_allowRealTimeSimulation;
bool m_hasGround;
@@ -1346,7 +1248,7 @@ struct PhysicsServerCommandProcessorInternalData
{
m_vrControllerEvents.init();
initHandles();
m_bodyHandles.initHandles();
#if 0
btAlignedObjectArray<int> bla;
@@ -1545,7 +1447,7 @@ void PhysicsServerCommandProcessor::processCollisionForces(btScalar timeStep)
if ((uid0<0)||(linkIndex<-1))
continue;
InternalBodyHandle* bodyHandle0 = m_data->getHandle(uid0);
InternalBodyHandle* bodyHandle0 = m_data->m_bodyHandles.getHandle(uid0);
SDFAudioSource* audioSrc = bodyHandle0->m_audioSources[linkIndex];
if (audioSrc==0)
continue;
@@ -1882,9 +1784,9 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
btRigidBody* rb = 0;
//get a body index
int bodyUniqueId = m_data->allocHandle();
int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
sd.m_bodyUniqueIds.push_back(bodyUniqueId);
@@ -2062,7 +1964,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
if (loadOk)
{
//get a body index
int bodyUniqueId = m_data->allocHandle();
int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
if (bodyUniqueIdPtr)
*bodyUniqueIdPtr= bodyUniqueId;
@@ -2075,7 +1977,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
}
u2b.setBodyUniqueId(bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
@@ -2258,7 +2160,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
int streamSizeInBytes = 0;
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
btMultiBody* mb = bodyHandle->m_multiBody;
if (mb)
{
@@ -2390,7 +2292,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID)&& (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0))
{
int bodyUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[0];
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (body)
{
if (body->m_multiBody)
@@ -2891,11 +2793,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
BT_PROFILE("CMD_SYNC_BODY_INFO");
int numHandles = m_data->getNumHandles();
int numHandles = m_data->m_bodyHandles.getNumHandles();
int actualNumBodies = 0;
for (int i=0;i<numHandles;i++)
{
InteralBodyData* body = m_data->getHandle(i);
InteralBodyData* body = m_data->m_bodyHandles.getHandle(i);
if (body && (body->m_multiBody || body->m_rigidBody))
{
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = i;
@@ -2929,7 +2831,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId;
serverStatusOut.m_dataStreamArguments.m_bodyName[0] = 0;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId);
if (bodyHandle)
{
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName,bodyHandle->m_bodyName.c_str());
@@ -2985,7 +2887,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
{
int bodyUniqueId = sd.m_bodyUniqueIds[i];
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (body)
{
if (body->m_multiBody)
@@ -3279,7 +3181,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
}
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
hasStatus = true;
@@ -3348,7 +3250,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
b3Printf("Processed CMD_CREATE_SENSOR");
}
int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
@@ -3418,7 +3320,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
@@ -3610,7 +3512,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
b3Printf("Sending the actual state (Q,U)");
}
int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
@@ -3852,13 +3754,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber);
}
///todo(erwincoumans) move this damping inside Bullet
for (int i=0;i<m_data->m_bodyHandles.size();i++)
{
applyJointDamping(i);
}
for (int i=0;i<m_data->m_dynamicsWorld->getNumMultibodies();i++)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i);
for (int l=0;l<mb->getNumLinks();l++)
{
for (int d=0;d<mb->getLink(l).m_dofCount;d++)
{
double damping_coefficient = mb->getLink(l).m_jointDamping;
double damping = -damping_coefficient*mb->getJointVelMultiDof(l)[d];
mb->addJointTorqueMultiDof(l, d, damping);
}
}
}
btScalar deltaTimeScaled = m_data->m_physicsDeltaTime*simTimeScalingFactor;
@@ -3987,7 +3895,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
b3Printf("Server Init Pose not implemented yet");
}
int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
btVector3 baseLinVel(0, 0, 0);
btVector3 baseAngVel(0, 0, 0);
@@ -4267,8 +4175,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_type = CMD_RIGID_BODY_CREATION_COMPLETED;
int bodyUniqueId = m_data->allocHandle();
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId;
rb->setUserIndex2(bodyUniqueId);
bodyHandle->m_rootLocalInertialFrame.setIdentity();
@@ -4557,7 +4465,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (bodyUniqueIdA >= 0)
{
InteralBodyData* bodyA = m_data->getHandle(bodyUniqueIdA);
InteralBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA);
if (bodyA)
{
if (bodyA->m_multiBody)
@@ -4591,7 +4499,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
if (bodyUniqueIdB>=0)
{
InteralBodyData* bodyB = m_data->getHandle(bodyUniqueIdB);
InteralBodyData* bodyB = m_data->m_bodyHandles.getHandle(bodyUniqueIdB);
if (bodyB)
{
if (bodyB->m_multiBody)
@@ -4745,7 +4653,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
BT_PROFILE("CMD_CALCULATE_INVERSE_DYNAMICS");
SharedMemoryStatus& serverCmd = serverStatusOut;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
@@ -4797,7 +4705,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
BT_PROFILE("CMD_CALCULATE_JACOBIAN");
SharedMemoryStatus& serverCmd = serverStatusOut;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
@@ -4860,7 +4768,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i)
{
InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);
InteralBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);
bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME) != 0);
if (body && body->m_multiBody)
@@ -4971,12 +4879,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT)
{
btScalar defaultMaxForce = 500.0;
InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
if (parentBody && parentBody->m_multiBody)
{
if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=-1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks())
{
InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
//also create a constraint with just a single multibody/rigid body without child
//if (childBody)
{
@@ -5189,7 +5097,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
@@ -5462,8 +5370,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btRigidBody* rb = btRigidBody::upcast(colObj);
if (rb)
{
int bodyUniqueId = m_data->allocHandle();
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
colObj->setUserIndex2(bodyUniqueId);
bodyHandle->m_rigidBody = rb;
@@ -5573,7 +5481,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR))
{
int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (body)
{
btCollisionObject* destColObj = 0;
@@ -5957,7 +5865,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const
if (gVRTrackingObjectUniqueId >= 0)
{
InternalBodyHandle* bodyHandle = m_data->getHandle(gVRTrackingObjectUniqueId);
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(gVRTrackingObjectUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
gVRTrackingObjectTr = bodyHandle->m_multiBody->getBaseWorldTransform();
@@ -5976,22 +5884,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const
}
}
void PhysicsServerCommandProcessor::applyJointDamping(int bodyUniqueId)
{
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
if (body) {
btMultiBody* mb = body->m_multiBody;
if (mb) {
for (int l=0;l<mb->getNumLinks();l++) {
for (int d=0;d<mb->getLink(l).m_dofCount;d++) {
double damping_coefficient = mb->getLink(l).m_jointDamping;
double damping = -damping_coefficient*mb->getJointVelMultiDof(l)[d];
mb->addJointTorqueMultiDof(l, d, damping);
}
}
}
}
}
void PhysicsServerCommandProcessor::resetSimulation()
{
@@ -6012,8 +5905,8 @@ void PhysicsServerCommandProcessor::resetSimulation()
deleteDynamicsWorld();
createEmptyDynamicsWorld();
m_data->exitHandles();
m_data->initHandles();
m_data->m_bodyHandles.exitHandles();
m_data->m_bodyHandles.initHandles();
m_data->m_hasGround = false;
m_data->m_gripperRigidbodyFixed = 0;
@@ -6038,7 +5931,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size(),0);
//loadUrdf("lego/lego.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_sphereId = bodyId;
InteralBodyData* parentBody = m_data->getHandle(bodyId);
InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setBaseVel(spawnDir * 5);
@@ -6059,7 +5952,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
if (loadUrdf("pr2_gripper.urdf", btVector3(-0.2, 0.15, 0.9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
{
InteralBodyData* parentBody = m_data->getHandle(bodyId);
InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setHasSelfCollision(0);
@@ -6089,7 +5982,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
m_data->m_KukaId = bodyId;
if (m_data->m_KukaId>=0)
{
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
InteralBodyData* kukaBody = m_data->m_bodyHandles.getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7)
{
btScalar q[7];
@@ -6127,7 +6020,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true,CUF_USE_SDF);
m_data->m_gripperId = bodyId + 1;
{
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
// Reset the default gripper motor maximum torque for damping to 0
for (int i = 0; i < gripperBody->m_multiBody->getNumLinks(); i++)
@@ -6165,7 +6058,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
if (m_data->m_gripperId>=0)
{
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
m_data->m_kukaGripperRevolute1 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 2, gripperBody->m_multiBody, 4, pivotInParent1, pivotInChild1);
m_data->m_kukaGripperRevolute1->setMaxAppliedImpulse(5.0);
m_data->m_kukaGripperRevolute2 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 3, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2);
@@ -6178,12 +6071,12 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
if (m_data->m_KukaId>=0)
{
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
InteralBodyData* kukaBody = m_data->m_bodyHandles.getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7)
{
if (m_data->m_gripperId>=0)
{
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
gripperBody->m_multiBody->setHasSelfCollision(0);
btVector3 pivotInParent(0, 0, 0.05);
@@ -6274,7 +6167,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
if (m_data->m_kukaGripperFixed && m_data->m_kukaGripperMultiBody)
{
InteralBodyData* childBody = m_data->getHandle(m_data->m_gripperId);
InteralBodyData* childBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
// Add gripper controller
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)childBody->m_multiBody->getLink(1).m_userPtr;
if (motor)
@@ -6342,7 +6235,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
// Inverse kinematics for KUKA
if (m_data->m_KukaId>=0)
{
InternalBodyHandle* bodyHandle = m_data->getHandle(m_data->m_KukaId);
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(m_data->m_KukaId);
if (bodyHandle && bodyHandle->m_multiBody && bodyHandle->m_multiBody->getNumDofs()==7)
{
btMultiBody* mb = bodyHandle->m_multiBody;