diff --git a/data/racecar/meshes/chassis_differential.STL b/data/racecar/meshes/chassis_differential.STL
new file mode 100644
index 000000000..793f6f197
Binary files /dev/null and b/data/racecar/meshes/chassis_differential.STL differ
diff --git a/data/racecar/racecar_differential.urdf b/data/racecar/racecar_differential.urdf
new file mode 100644
index 000000000..8f5f37519
--- /dev/null
+++ b/data/racecar/racecar_differential.urdf
@@ -0,0 +1,743 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ EffortJointInterface
+
+
+ EffortJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ EffortJointInterface
+
+
+ EffortJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ EffortJointInterface
+
+
+ EffortJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ EffortJointInterface
+
+
+ EffortJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index d3a62ec52..cbddd1ddc 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -3181,24 +3181,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_requestPixelDataArguments.m_projectionMatrix);
} else
{
- SharedMemoryStatus tmpCmd = serverStatusOut;
+ b3OpenGLVisualizerCameraInfo tmpCamResult;
bool result = this->m_data->m_guiHelper->getCameraInfo(
- &tmpCmd.m_visualizerCameraResultArgs.m_width,
- &tmpCmd.m_visualizerCameraResultArgs.m_height,
- tmpCmd.m_visualizerCameraResultArgs.m_viewMatrix,
- tmpCmd.m_visualizerCameraResultArgs.m_projectionMatrix,
- tmpCmd.m_visualizerCameraResultArgs.m_camUp,
- tmpCmd.m_visualizerCameraResultArgs.m_camForward,
- tmpCmd.m_visualizerCameraResultArgs.m_horizontal,
- tmpCmd.m_visualizerCameraResultArgs.m_vertical,
- &tmpCmd.m_visualizerCameraResultArgs.m_yaw,
- &tmpCmd.m_visualizerCameraResultArgs.m_pitch,
- &tmpCmd.m_visualizerCameraResultArgs.m_dist,
- tmpCmd.m_visualizerCameraResultArgs.m_target);
+ &tmpCamResult.m_width,
+ &tmpCamResult.m_height,
+ tmpCamResult.m_viewMatrix,
+ tmpCamResult.m_projectionMatrix,
+ tmpCamResult.m_camUp,
+ tmpCamResult.m_camForward,
+ tmpCamResult.m_horizontal,
+ tmpCamResult.m_vertical,
+ &tmpCamResult.m_yaw,
+ &tmpCamResult.m_pitch,
+ &tmpCamResult.m_dist,
+ tmpCamResult.m_target);
if (result)
{
- m_data->m_visualConverter.render(tmpCmd.m_visualizerCameraResultArgs.m_viewMatrix,
- tmpCmd.m_visualizerCameraResultArgs.m_projectionMatrix);
+ m_data->m_visualConverter.render(tmpCamResult.m_viewMatrix,
+ tmpCamResult.m_projectionMatrix);
} else
{
m_data->m_visualConverter.render();
diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
index 143c931ce..c8b1b2221 100644
--- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
+++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
@@ -686,17 +686,19 @@ void TinyRendererVisualShapeConverter::changeRGBAColor(int bodyUniqueId, int lin
break;
}
}
-
- TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(start);
- TinyRendererObjectArray* visualArray = *visualArrayPtr;
-
- btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(start);
- const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
-
- float rgba[4] = {rgbaColor[0], rgbaColor[1], rgbaColor[2], rgbaColor[3]};
- for (int v=0;vm_renderObjects.size();v++)
+ if (start>=0)
{
- visualArray->m_renderObjects[v]->m_model->setColorRGBA(rgba);
+ TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(start);
+ TinyRendererObjectArray* visualArray = *visualArrayPtr;
+
+ btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(start);
+ const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
+
+ float rgba[4] = {rgbaColor[0], rgbaColor[1], rgbaColor[2], rgbaColor[3]};
+ for (int v=0;vm_renderObjects.size();v++)
+ {
+ visualArray->m_renderObjects[v]->m_model->setColorRGBA(rgba);
+ }
}
}
diff --git a/examples/pybullet/examples/racecar_differential.py b/examples/pybullet/examples/racecar_differential.py
new file mode 100644
index 000000000..251ce0141
--- /dev/null
+++ b/examples/pybullet/examples/racecar_differential.py
@@ -0,0 +1,71 @@
+import pybullet as p
+import time
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid<0):
+ p.connect(p.GUI)
+
+p.resetSimulation()
+p.setGravity(0,0,-10)
+p.setPhysicsEngineParameter(numSolverIterations=1000)
+useRealTimeSim = 1
+
+#for video recording (works best on Mac and Linux, not well on Windows)
+#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
+p.setRealTimeSimulation(useRealTimeSim) # either this
+#p.loadURDF("plane.urdf")
+p.loadSDF("stadium.sdf")
+
+car = p.loadURDF("racecar/racecar_differential.urdf")#, [0,0,2],useFixedBase=True)
+for i in range (p.getNumJoints(car)):
+ print (p.getJointInfo(car,i))
+for wheel in range(p.getNumJoints(car)):
+ p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
+ p.getJointInfo(car,wheel)
+
+wheels = [8,15]
+print("----------------")
+
+#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
+
+c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=1, maxForce=10000)
+
+c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=1, maxForce=10000)
+
+c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+
+
+steering = [0,2]
+
+targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-50,50,0)
+maxForceSlider = p.addUserDebugParameter("maxForce",0,50,20)
+steeringSlider = p.addUserDebugParameter("steering",-1,1,0)
+while (True):
+ maxForce = p.readUserDebugParameter(maxForceSlider)
+ targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
+ steeringAngle = p.readUserDebugParameter(steeringSlider)
+ #print(targetVelocity)
+
+ for wheel in wheels:
+ p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=maxForce)
+
+ for steer in steering:
+ p.setJointMotorControl2(car,steer,p.POSITION_CONTROL,targetPosition=-steeringAngle)
+
+ steering
+ if (useRealTimeSim==0):
+ p.stepSimulation()
+ time.sleep(0.01)
diff --git a/examples/pybullet/gym/envs/bullet/kuka.py b/examples/pybullet/gym/envs/bullet/kuka.py
index da6b53444..a8dc4b7d3 100644
--- a/examples/pybullet/gym/envs/bullet/kuka.py
+++ b/examples/pybullet/gym/envs/bullet/kuka.py
@@ -72,9 +72,10 @@ class Kuka:
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
pos = state[0]
orn = state[1]
+ euler = p.getEulerFromQuaternion(orn)
observation.extend(list(pos))
- observation.extend(list(orn))
+ observation.extend(list(euler))
return observation
diff --git a/examples/pybullet/gym/envs/bullet/kukaGymEnv.py b/examples/pybullet/gym/envs/bullet/kukaGymEnv.py
index c76d69044..b0a3da4d9 100644
--- a/examples/pybullet/gym/envs/bullet/kukaGymEnv.py
+++ b/examples/pybullet/gym/envs/bullet/kukaGymEnv.py
@@ -31,6 +31,7 @@ class KukaGymEnv(gym.Env):
self._p = p
if self._renders:
p.connect(p.GUI)
+ p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
else:
p.connect(p.DIRECT)
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
@@ -52,8 +53,7 @@ class KukaGymEnv(gym.Env):
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1])
- if self._renders:
- p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
+
p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
xpos = 0.5 +0.05*random.random()
@@ -78,14 +78,21 @@ class KukaGymEnv(gym.Env):
def getExtendedObservation(self):
self._observation = self._kuka.getObservation()
- pos,orn = p.getBasePositionAndOrientation(self.blockUid)
- self._observation.extend(list(pos))
- self._observation.extend(list(orn))
-
+ eeState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
+ endEffectorPos = eeState[0]
+ endEffectorOrn = eeState[1]
+ blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)
+
+ invEEPos,invEEOrn = p.invertTransform(endEffectorPos,endEffectorOrn)
+ blockPosInEE,blockOrnInEE = p.multiplyTransforms(invEEPos,invEEOrn,blockPos,blockOrn)
+ blockEulerInEE = p.getEulerFromQuaternion(blockOrnInEE)
+ self._observation.extend(list(blockPosInEE))
+ self._observation.extend(list(blockEulerInEE))
+
return self._observation
def _step(self, action):
- dv = 0.002
+ dv = 0.01
dx = [0,-dv,dv,0,0,0,0][action]
dy = [0,0,0,-dv,dv,0,0][action]
da = [0,0,0,0,0,-0.1,0.1][action]