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

Merge pull request #886 from erwincoumans/master

Browse Source 
some VR tweaks and bugfix for issue #878
This commit is contained in:
erwincoumans
2016-12-11 12:53:47 -08:00
committed by GitHub
parent e8f38df121 436fdd5dc2
commit 0c26fee04d
14 changed files with 178 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
data/gripper/wsg50_one_motor_gripper_free_base.sdf
View File

@@ -303,6 +303,10 @@
</joint> </joint>
<link name='finger_right'> <link name='finger_right'>
<contact>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<pose frame=''>0.042 0 0.145 0 0 1.5708</pose> <pose frame=''>0.042 0 0.145 0 0 1.5708</pose>
<inertial> <inertial>
<mass>0.2</mass> <mass>0.2</mass>
@@ -343,6 +347,10 @@
</joint> </joint>
<link name='finger_left'> <link name='finger_left'>
<contact>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<pose frame=''>-0.042 0 0.145 0 0 4.71239</pose> <pose frame=''>-0.042 0 0.145 0 0 4.71239</pose>
<inertial> <inertial>
<mass>0.2</mass> <mass>0.2</mass>

8
data/gripper/wsg50_one_motor_gripper_new_free_base.sdf
View File

@@ -300,6 +300,10 @@
</joint> </joint>
<link name='finger_right'> <link name='finger_right'>
<contact>
<lateral_friction>1.0</lateral_friction>
<spinning_friction>1.5</spinning_friction>
</contact>
<pose frame=''>0.062 0 0.145 0 0 1.5708</pose> <pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
<inertial> <inertial>
<mass>0.2</mass> <mass>0.2</mass>
@@ -340,6 +344,10 @@
</joint> </joint>
<link name='finger_left'> <link name='finger_left'>
<contact>
<lateral_friction>1.0</lateral_friction>
<spinning_friction>1.5</spinning_friction>
</contact>
<pose frame=''>-0.062 0 0.145 0 0 4.71239</pose> <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
<inertial> <inertial>
<mass>0.2</mass> <mass>0.2</mass>

100
data/quadruped/quadruped.urdf
View File

@@ -5,8 +5,8 @@
<geometry> <geometry>
<box size=".226 0.16 .07"/> <box size=".226 0.16 .07"/>
</geometry> </geometry>
<material name="blue"> <material name="black">
<color rgba="0 0 .8 1"/> <color rgba="0 0 0 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -26,9 +26,7 @@
<geometry> <geometry>
<cylinder length="0.026" radius="0.0434"/> <cylinder length="0.026" radius="0.0434"/>
</geometry> </geometry>
<material name="blue"> <material name="black"/>
<color rgba="0 0 .8 1"/>
</material>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
@@ -55,9 +53,7 @@
<geometry> <geometry>
<cylinder length="0.026" radius="0.0434"/> <cylinder length="0.026" radius="0.0434"/>
</geometry> </geometry>
<material name="blue"> <material name="black"/>
<color rgba="0 0 .8 1"/>
</material>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
@@ -84,9 +80,7 @@
<geometry> <geometry>
<cylinder length="0.026" radius="0.0434"/> <cylinder length="0.026" radius="0.0434"/>
</geometry> </geometry>
<material name="blue"> <material name="black"/>
<color rgba="0 0 .8 1"/>
</material>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
@@ -113,9 +107,7 @@
<geometry> <geometry>
<cylinder length="0.026" radius="0.0434"/> <cylinder length="0.026" radius="0.0434"/>
</geometry> </geometry>
<material name="blue"> <material name="black"/>
<color rgba="0 0 .8 1"/>
</material>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
@@ -142,9 +134,7 @@
<geometry> <geometry>
<cylinder length="0.026" radius="0.0434"/> <cylinder length="0.026" radius="0.0434"/>
</geometry> </geometry>
<material name="blue"> <material name="black"/>
<color rgba="0 0 .8 1"/>
</material>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
@@ -171,9 +161,7 @@
<geometry> <geometry>
<cylinder length="0.026" radius="0.0434"/> <cylinder length="0.026" radius="0.0434"/>
</geometry> </geometry>
<material name="blue"> <material name="black"/>
<color rgba="0 0 .8 1"/>
</material>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
@@ -200,9 +188,7 @@
<geometry> <geometry>
<cylinder length="0.026" radius="0.0434"/> <cylinder length="0.026" radius="0.0434"/>
</geometry> </geometry>
<material name="blue"> <material name="black"/>
<color rgba="0 0 .8 1"/>
</material>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
@@ -229,9 +215,7 @@
<geometry> <geometry>
<cylinder length="0.026" radius="0.0434"/> <cylinder length="0.026" radius="0.0434"/>
</geometry> </geometry>
<material name="blue"> <material name="black"/>
<color rgba="0 0 .8 1"/>
</material>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
@@ -258,8 +242,8 @@
<geometry> <geometry>
<box size=".01 0.01 .11"/> <box size=".01 0.01 .11"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -286,8 +270,8 @@
<geometry> <geometry>
<box size=".01 0.01 .11"/> <box size=".01 0.01 .11"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -315,8 +299,8 @@
<geometry> <geometry>
<box size=".01 0.01 .2"/> <box size=".01 0.01 .2"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -345,8 +329,8 @@
<geometry> <geometry>
<box size=".01 0.01 .2"/> <box size=".01 0.01 .2"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -374,8 +358,8 @@
<geometry> <geometry>
<box size=".01 0.01 .11"/> <box size=".01 0.01 .11"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -402,8 +386,8 @@
<geometry> <geometry>
<box size=".01 0.01 .11"/> <box size=".01 0.01 .11"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -431,8 +415,8 @@
<geometry> <geometry>
<box size=".01 0.01 .2"/> <box size=".01 0.01 .2"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -461,8 +445,8 @@
<geometry> <geometry>
<box size=".01 0.01 .2"/> <box size=".01 0.01 .2"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -493,8 +477,8 @@
<geometry> <geometry>
<box size=".01 0.01 .11"/> <box size=".01 0.01 .11"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -521,8 +505,8 @@
<geometry> <geometry>
<box size=".01 0.01 .11"/> <box size=".01 0.01 .11"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -550,8 +534,8 @@
<geometry> <geometry>
<box size=".01 0.01 .2"/> <box size=".01 0.01 .2"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -580,8 +564,8 @@
<geometry> <geometry>
<box size=".01 0.01 .2"/> <box size=".01 0.01 .2"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -609,8 +593,8 @@
<geometry> <geometry>
<box size=".01 0.01 .11"/> <box size=".01 0.01 .11"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -637,8 +621,8 @@
<geometry> <geometry>
<box size=".01 0.01 .11"/> <box size=".01 0.01 .11"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -666,8 +650,8 @@
<geometry> <geometry>
<box size=".01 0.01 .2"/> <box size=".01 0.01 .2"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
@@ -696,8 +680,8 @@
<geometry> <geometry>
<box size=".01 0.01 .2"/> <box size=".01 0.01 .2"/>
</geometry> </geometry>
<material name="blue"> <material name="grey">
<color rgba="0 0 .8 1"/> <color rgba="0.65 0.65 0.75 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>

53
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -48,7 +48,7 @@ int gHuskyId = -1;
btTransform huskyTr = btTransform::getIdentity(); btTransform huskyTr = btTransform::getIdentity();
int gCreateObjectSimVR = -1; int gCreateObjectSimVR = -1;
int gEnableKukaControl = 1; int gEnableKukaControl = 0;
btScalar simTimeScalingFactor = 1; btScalar simTimeScalingFactor = 1;
btScalar gRhsClamp = 1.f; btScalar gRhsClamp = 1.f;
@@ -667,8 +667,6 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
m_data = new PhysicsServerCommandProcessorInternalData(); m_data = new PhysicsServerCommandProcessorInternalData();
createEmptyDynamicsWorld(); createEmptyDynamicsWorld();
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
} }
@@ -717,6 +715,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08; m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
} }
void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies() void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()
@@ -4030,7 +4031,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
} }
btVector3 gVRGripperPos(0,0,0.2); btVector3 gVRGripperPos(0.6, 0.4, 0.7);
btQuaternion gVRGripperOrn(0,0,0,1); btQuaternion gVRGripperOrn(0,0,0,1);
btVector3 gVRController2Pos(0,0,0.2); btVector3 gVRController2Pos(0,0,0.2);
btQuaternion gVRController2Orn(0,0,0,1); btQuaternion gVRController2Orn(0,0,0,1);
@@ -4044,6 +4045,12 @@ int gDroppedSimulationSteps = 0;
int gNumSteps = 0; int gNumSteps = 0;
double gDtInSec = 0.f; double gDtInSec = 0.f;
double gSubStep = 0.f; double gSubStep = 0.f;
void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTimeSim)
{
m_data->m_allowRealTimeSimulation = enableRealTimeSim;
}
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{ {
if (gResetSimulation) if (gResetSimulation)
@@ -4052,7 +4059,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
gResetSimulation = false; gResetSimulation = false;
} }
if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper) if ((m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
{ {
///this hardcoded C++ scene creation is temporary for demo purposes. It will be done in Python later... ///this hardcoded C++ scene creation is temporary for demo purposes. It will be done in Python later...
@@ -4170,7 +4177,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{ {
int bodyId = 0; int bodyId = 0;
if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size())) 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->getHandle(bodyId);
if (parentBody->m_multiBody) if (parentBody->m_multiBody)
@@ -4200,6 +4207,30 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_KukaId = bodyId; m_data->m_KukaId = bodyId;
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7)
{
btScalar q[7];
q[0] = 0;// -SIMD_HALF_PI;
q[1] = 0;
q[2] = 0;
q[3] = SIMD_HALF_PI;
q[4] = 0;
q[5] = -SIMD_HALF_PI*0.66;
q[6] = 0;
for (int i = 0; i < 7; i++)
{
kukaBody->m_multiBody->setJointPos(i, q[i]);
}
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m);
int nLinks = kukaBody->m_multiBody->getNumLinks();
scratch_q.resize(nLinks + 1);
scratch_m.resize(nLinks + 1);
kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
}
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -4208,7 +4239,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
// Load one motor gripper for kuka // Load one motor gripper for kuka
loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
m_data->m_gripperId = bodyId + 1; m_data->m_gripperId = bodyId + 1;
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId); InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
// Reset the default gripper motor maximum torque for damping to 0 // Reset the default gripper motor maximum torque for damping to 0
@@ -4250,6 +4281,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1); m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2); m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2);
kukaBody = m_data->getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7) if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7)
{ {
gripperBody->m_multiBody->setHasSelfCollision(0); gripperBody->m_multiBody->setHasSelfCollision(0);
@@ -4303,7 +4335,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
// Table area // Table area
loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); //loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); //loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); //loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -4344,9 +4376,10 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
if (motor) if (motor)
{ {
btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75; btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75;
motor->setPositionTarget(posTarget, .2); motor->setPositionTarget(posTarget, .8);
motor->setVelocityTarget(0.0, .5); motor->setVelocityTarget(0.0, .5);
motor->setMaxAppliedImpulse(5.0); motor->setMaxAppliedImpulse(1.0);
} }
} }

1
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -85,6 +85,7 @@ public:
void replayFromLogFile(const char* fileName); void replayFromLogFile(const char* fileName);
void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes ); void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
void stepSimulationRealTime(double dtInSec); void stepSimulationRealTime(double dtInSec);
void enableRealTimeSimulation(bool enableRealTimeSim);
void applyJointDamping(int bodyUniqueId); void applyJointDamping(int bodyUniqueId);
}; };

62
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -27,6 +27,7 @@
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet! //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
extern btVector3 gLastPickPos; extern btVector3 gLastPickPos;
btVector3 gVRTeleportPos1(0,0,0); btVector3 gVRTeleportPos1(0,0,0);
btScalar gVRTeleportRotZ = 0;
btQuaternion gVRTeleportOrn(0, 0, 0,1); btQuaternion gVRTeleportOrn(0, 0, 0,1);
extern btVector3 gVRGripperPos; extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn; extern btQuaternion gVRGripperOrn;
@@ -46,10 +47,47 @@ extern btScalar simTimeScalingFactor;
extern bool gVRGripperClosed; extern bool gVRGripperClosed;
#if B3_USE_MIDI const char* startFileNameVR = "0_VRDemoSettings.txt";
#include <vector> #include <vector>
//remember the settings (you don't want to re-tune again and again...)
static void saveCurrentSettingsVR()
{
FILE* f = fopen(startFileNameVR, "w");
if (f)
{
fprintf(f, "--camPosX= %f\n", gVRTeleportPos1[0]);
fprintf(f, "--camPosY= %f\n", gVRTeleportPos1[1]);
fprintf(f, "--camPosZ= %f\n", gVRTeleportPos1[2]);
fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ);
fclose(f);
}
};
static void loadCurrentSettingsVR(b3CommandLineArgs& args)
{
int currentEntry = 0;
FILE* f = fopen(startFileNameVR, "r");
if (f)
{
char oneline[1024];
char* argv[] = { 0,&oneline[0] };
while (fgets(oneline, 1024, f) != NULL)
{
char *pos;
if ((pos = strchr(oneline, '\n')) != NULL)
*pos = '\0';
args.addArgs(2, argv);
}
fclose(f);
}
};
#if B3_USE_MIDI
static float getParamf(float rangeMin, float rangeMax, int midiVal) static float getParamf(float rangeMin, float rangeMax, int midiVal)
{ {
float v = rangeMin + (rangeMax - rangeMin)* (float(midiVal / 127.)); float v = rangeMin + (rangeMax - rangeMin)* (float(midiVal / 127.));
@@ -70,9 +108,10 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
{ {
if (message->at(1) == 16) if (message->at(1) == 16)
{ {
float rotZ = getParamf(-3.1415, 3.1415, message->at(2)); gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2));
gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), rotZ); gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
b3Printf("gVRTeleportOrn rotZ = %f\n", rotZ); saveCurrentSettingsVR();
b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ);
} }
if (message->at(1) == 32) if (message->at(1) == 32)
@@ -85,6 +124,7 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
if (message->at(1) == i) if (message->at(1) == i)
{ {
gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2)); gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2));
saveCurrentSettingsVR();
b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPos1[i]); b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPos1[i]);
} }
@@ -927,7 +967,7 @@ public:
virtual void processCommandLineArgs(int argc, char* argv[]) virtual void processCommandLineArgs(int argc, char* argv[])
{ {
b3CommandLineArgs args(argc,argv); b3CommandLineArgs args(argc,argv);
loadCurrentSettingsVR(args);
if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0])) if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0]))
{ {
printf("camPosX=%f\n", gVRTeleportPos1[0]); printf("camPosX=%f\n", gVRTeleportPos1[0]);
@@ -1544,7 +1584,11 @@ void PhysicsServerExample::renderScene()
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{ {
gEnableRealTimeSimVR = true; if (!gEnableRealTimeSimVR)
{
gEnableRealTimeSimVR = true;
m_physicsServer.enableRealTimeSimulation(1);
}
} }
@@ -1665,8 +1709,10 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
return; return;
if (gGraspingController < 0) if (gGraspingController < 0)
{
gGraspingController = controllerId; gGraspingController = controllerId;
gEnableKukaControl = true;
}
if (controllerId != gGraspingController) if (controllerId != gGraspingController)
{ {
if (button == 1 && state == 0) if (button == 1 && state == 0)
@@ -1777,8 +1823,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis) void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis)
{ {
gEnableRealTimeSimVR = true;
if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS) if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS)
{ {
printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS); printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);

6
examples/SharedMemory/PhysicsServerSharedMemory.cpp
View File

@@ -241,6 +241,12 @@ void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec)
m_data->m_commandProcessor->stepSimulationRealTime(dtInSec); m_data->m_commandProcessor->stepSimulationRealTime(dtInSec);
} }
void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim)
{
m_data->m_commandProcessor->enableRealTimeSimulation(enableRealTimeSim);
}
void PhysicsServerSharedMemory::processClientCommands() void PhysicsServerSharedMemory::processClientCommands()
{ {

2
examples/SharedMemory/PhysicsServerSharedMemory.h
View File

@@ -28,6 +28,8 @@ public:
virtual void stepSimulationRealTime(double dtInSec); virtual void stepSimulationRealTime(double dtInSec);
virtual void enableRealTimeSimulation(bool enableRealTimeSim);
//bool supportsJointMotor(class btMultiBody* body, int linkIndex); //bool supportsJointMotor(class btMultiBody* body, int linkIndex);

2
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@@ -855,7 +855,7 @@ void CMainApplication::RenderFrame()
// SwapWindow // SwapWindow
{ {
B3_PROFILE("m_app->swapBuffer"); B3_PROFILE("m_app->swapBuffer");
// m_app->swapBuffer(); m_app->swapBuffer();
//SDL_GL_SwapWindow( m_pWindow ); //SDL_GL_SwapWindow( m_pWindow );
} }

2
examples/TinyRenderer/TinyRenderer.h
View File

@@ -33,7 +33,7 @@ struct TinyRenderObjectData
TGAImage& m_rgbColorBuffer; TGAImage& m_rgbColorBuffer;
b3AlignedObjectArray<float>& m_depthBuffer;//required, hence a reference b3AlignedObjectArray<float>& m_depthBuffer;//required, hence a reference
b3AlignedObjectArray<float>* m_shadowBuffer; b3AlignedObjectArray<float>* m_shadowBuffer;//optional, hence a pointer
b3AlignedObjectArray<int>* m_segmentationMaskBufferPtr;//optional, hence a pointer b3AlignedObjectArray<int>* m_segmentationMaskBufferPtr;//optional, hence a pointer
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer); TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer);

1
examples/TinyRenderer/main.cpp
View File

@@ -147,7 +147,6 @@ int main(int argc, char* argv[])
renderData.m_rgbColorBuffer.set(x,y,color); renderData.m_rgbColorBuffer.set(x,y,color);
renderData.m_depthBuffer[x+y*textureWidth] = -1e30f; renderData.m_depthBuffer[x+y*textureWidth] = -1e30f;
renderData.m_shadowBuffer[x+y*textureWidth] = -1e30f;
} }
} }

10
examples/pybullet/pybullet.c
View File

@@ -432,7 +432,7 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
return NULL; return NULL;
} }
static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold","numSubSteps", NULL }; static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidi", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps)) if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidi", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps))
{ {
@@ -446,6 +446,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
{ {
b3PhysicsParamSetNumSolverIterations(command, numSolverIterations); b3PhysicsParamSetNumSolverIterations(command, numSolverIterations);
} }
if (numSubSteps >= 0)
{
b3PhysicsParamSetNumSubSteps(command, numSubSteps);
}
if (fixedTimeStep >= 0) if (fixedTimeStep >= 0)
{ {
b3PhysicsParamSetTimeStep(command, fixedTimeStep); b3PhysicsParamSetTimeStep(command, fixedTimeStep);
@@ -458,10 +462,6 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
{ {
b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, splitImpulsePenetrationThreshold); b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, splitImpulsePenetrationThreshold);
} }
if (numSubSteps>=0)
{
b3PhysicsParamSetNumSubSteps(command,numSubSteps);
}
//ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); //ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);

12
examples/pybullet/quadruped.py
View File

@@ -4,9 +4,10 @@ import math
p.connect(p.SHARED_MEMORY) p.connect(p.SHARED_MEMORY)
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3) p.setGravity(0,0,-1)
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/quadruped.urdf",10,-2,2)
#p.getNumJoints(1) #p.getNumJoints(1)
#right front leg #right front leg
p.resetJointState(quadruped,0,1.57) p.resetJointState(quadruped,0,1.57)
p.resetJointState(quadruped,2,-2.2) p.resetJointState(quadruped,2,-2.2)
@@ -64,16 +65,17 @@ p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,22,p.VELOCITY_CONTROL,0,0) p.setJointMotorControl(quadruped,22,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0) p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0)
p.setGravity(0,0,-10)
p_gain = 2 p_gain = 2
speed = 10 speed = 10
amplitude = 1.3 amplitude = 1.3
#stand still #stand still
t_end = time.time() + 5 t_end = time.time() + 2
while time.time() < t_end: while time.time() < t_end:
p.stepSimulation() p.stepSimulation()
p.setGravity(0,0,-10)
jump_amp = 0.5 jump_amp = 0.5
@@ -125,4 +127,4 @@ while time.time() < t_end:
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1) p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1)
p.stepSimulation() p.stepSimulation()
p.setRealTimeSimulation(1)

2
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -570,7 +570,7 @@ void btMultiBody::clearForcesAndTorques()
void btMultiBody::clearVelocities() void btMultiBody::clearVelocities()
{ {
for (int i = 0; i < 6 + getNumLinks(); ++i) for (int i = 0; i < 6 + getNumDofs(); ++i)
{ {
m_realBuf[i] = 0.f; m_realBuf[i] = 0.f;
} }