From 71170d638455b34ddbea6ef12946aa6e6fb9f43b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 20 Jun 2017 20:22:14 -0700 Subject: [PATCH] reduce stack usage (cause some crashes in low-stack tests) fix crashing bug in changeVisualShape add differential gear version of racecar (only 2 back wheels are powered) --- data/racecar/meshes/chassis_differential.STL | Bin 0 -> 20284 bytes data/racecar/racecar_differential.urdf | 743 ++++++++++++++++++ .../PhysicsServerCommandProcessor.cpp | 30 +- .../TinyRendererVisualShapeConverter.cpp | 22 +- .../pybullet/examples/racecar_differential.py | 71 ++ examples/pybullet/gym/envs/bullet/kuka.py | 3 +- .../pybullet/gym/envs/bullet/kukaGymEnv.py | 21 +- 7 files changed, 857 insertions(+), 33 deletions(-) create mode 100644 data/racecar/meshes/chassis_differential.STL create mode 100644 data/racecar/racecar_differential.urdf create mode 100644 examples/pybullet/examples/racecar_differential.py diff --git a/data/racecar/meshes/chassis_differential.STL b/data/racecar/meshes/chassis_differential.STL new file mode 100644 index 0000000000000000000000000000000000000000..793f6f197429857522f7133447a9bce9b7b93230 GIT binary patch literal 20284 zcmb8153pQyb;oa%fTB$U4O1HVlaN}2GcYlE?+Na{-DnD7=rBXHq<;p~p#~_5M5vV} z{8=OkQz;3Rs4awlePI672AaGVfwwP9O+wTm9cZHgr6Uk+iquwZBifOq=bYa;_kQl( z-8bXT7>;|+_xC;je&_!7?z?;54a={+?$4HeY}OUmU3<-}4}4Lo2jB95$(OGCY~##J*O{DU zzddisg7v4pX&}p5O==ztanrF^RT}I4#*OsxXKX8hmT1{#g$F>)y4Bsm;Y2M29xMclkZ1Hmf5mw>NJ5<(q-naZJs!s~jstvsTva!RLtm|9JY) znwc+*9X)zWlN?-^E0yD5uD0HD>O}O_gU`QhBKob2U?;N223FkAm{>42QPeyl_jS!* zX4*MCj*Z*CxjhiNuH?toq7~6u_Ttl{qd#7cQdr!!+)@$v(9m@r6%)Jg)~eM0gf@a1cQ&&w2%cdYFcY-hEX? z(8{~gIqv<$Pv=K=sp)~3NR#2u4{Wi09vm?T9c@-8y?tHdvRN;;i(Pxsk=3@(RiiWU zPO15Mzj&na7d|;z%`E%*u557a8(%jBz2drDDMfJcljPghoyTtvI!_?6g1n_57;l-Dml6w8_2;JiA5rw&H5g zW#zqFL8w>Ma9!U8ju^jxW9-@vG;JW-@UUI@B)RIK9$_^sonf+9QxFf%ygg=OCu*Qp zDt~y^%Q~ssTq(;AUU{R`JIf@FE{<52D-~WT0{gs;z1_y1M;yW7vrl}_r&@TWxe~1%#> z`|ulQXzzC7q^BA^C%z`vLD_eeI0}}W!{VTaWey^!As_aVz!rRzE!gOjmGz^cQp$| z1#i1x%Fs1uZ5xbwgl5f|=ML+cTI)gXEc>6^pUS(0>|D2u_|wlX-!}jF59x&KP(u#( zSe8Az=$3q+Q^T5bC98+Tps!X3SD7}hYHeH}r5?qrRoa8xceu_AV&;J(xmOx_>_L1g zD(%OD0bW_}5`j~#a7bTO<1hrRa4ta{=qp2beoV$$OAt#w^;#YWHAJxU%W)7vEAM0D zD-h}xHAHX{CB(et|0p=|>{dW|u=WSl8ah4E{r5oy&U5%U?BKlv? zso8z^w!E79nKLRY^>}H@(D`>iqi3HEHA}_no0mPNC%s9owe*U7T&d`G%@q-}^7k$4 z3Gt=(SM0hpOvHXeqH~+zFHmOU>tJKsz89R zmh6Lr8pKosH#j6$j6?5>MSg5u&u)dIx>i`Bvf%iQ2i`MQ)dWKLYATD@&Y@kR5mBSs zgqTySuB5iD&a}IJHbe~(;;o_8GQEO>2wLHN>Gt$8jvdqx0lx79aluW828o~*;xk06 z88}V`?s>d#+3kto{XNdBgs{vikFdx6PdSU4JfZUg@ApS&mnfPDl?|b*C(k5W?WJL{ zj$LF^Lh@$si{Tu#w#i`8QzLTRPZFH1TxjPC^w1T=PELxzEU75Qv z88q4w0^TCKm?egw72=pAyPEryw>e#z*40`~+2^qbtp-F$9EMiQ^wP*xLgd;@xfR|= z?#vAh)J#n#Y*8>t9j?Y?PDBuBCfdN%btai3J2Gs6|%r75COjNA`X>^pcOI#j@Bus%T+DUdz%Pk!8x>d zr5>z#n^vfeA&RS-dgY&G>hREHe#YsE;HNvHEsie4gkJlcBbWUOXT94K5$om%|72+h z&1-(@({8Yn9ysb=8#%qsDRR|O95qBhF6vf>6pqKAc7((N+at=o-* z2=8P4{DC+yeGEaX{*fbB8S@y@g@BV)0JLgWb z696@k>$t-z9CHQngB}Q4K^Bhho;$50JjOu{p*@RtPRv+o9MnLT z(-}|5KN3W{2ZC0Rg~RKy6ZOyv>Zvb3>3iP1OS~Rk2wFjw$h{smuc(0>_pF`6>(Pbq z?3xwiBD;p526Ef;<{Vy+E(EP03y0Uk5Y#}{Q>K1;aSpFX7lKxhg=17YU2CHC@KdU; zRfin+tes=#(~BEV_CWA1Fl6EQo^<-z((%ec4dl2N?i^1EVo?tS?>a*k4yj;Mc9d~Y z1G&w+-2D->f-D^WESbx%sSvY=8>T$yRQaz}F9N#W_ z%`X(hs86v!EF7Dp9=GpJ*`)?@+?j9=nI(BWU={Z=%&L6%!|TyZ`=dCEdOe19 zuXsjG&g;>IpcUkHzCR2>4dlqQb9g=gq7j3y0UE zp}C?4a%9>$ydGT$+>IhaIEe6_V{1WT?g35T)$W z3TX9e)hgSuhtB^@NAYo&6CPUL{NADtVfF9>44WL3N4$AX}S2;qxv7libc zAvDWG&`M?RD?v!EtOKR5G>%+DgyhXQ?h>-dlN18rTbMs2G zOa!e|R*r-K$4+X90N=RvRdAFLv;wyvkgE`5NmApG`Q8&Ac*%dgbfd9S^kyMz7%DPfQU^3Xe3Jnobf8*$yJ|zULRMwRe0vtAdLPG@jYNaV! zLeL6v2%MgR5EzQMR%Pr+p;|TsceAhX~=Td0(kl_L4jF>D*rP z$ZZ$7?=>}1IoYOv(>;A^blpJw$sNb(_1wF?AA(lMi18`LywY=ftgH1156$-;UKQtd z^Op$8k)cax(N0#JUJXGjpetT!O;dz+ZZ}!qV@6Wk7Ke-BFQUy6se;L{JG-&2S}`Nw zNayzOL}y}nXubzG5S7S@P##63J(zn{$u6ycwwTi2U!5Rc={F6YtD1gqpt4xk^m~II zUI`gBy%54{F{xiES44!YR{D)V@TDl?TN$it-&DJYUip9D$c-W(Y8plb} zSB#tp@y6mS`pOZd?9vKo@#^P(t$kIdXmDtR{6316m8*e&9y}`NcWQ{>cVYbzv_b{N zt6lObLi6!L95=Vm?BEVPckDwL26~wEat6$20sUZS+6T}Z3 zF=c8QK`Ufk5dT+Hut^OOT3P)DTPiDvxT@(VNFr#3UJ%4j{Cp){(xiq6?P=xENo0Lx zSLZ5%R@?>FC3cOKzG_lKg!XQVLr+W6i9HdtLbnUzfBjR$Ly|LUh|s;F9BD@d;^7K{ zRx0a0FC&CQPfNLm2=I*;UI|ADK`VGG27d*4H3wjAf&GhA$?Us&g!ENZ zutNWR#p(`k`jVe-Z73qC)&J9 zag4D(m}YL`^FcR0->r>JjKzKgA(u(KuP!_fCjOK1bQr&ihJn%7N3XA!r43 zE%7Kq&+V8vrHpoaCFgdFgBr||Qf(rZZbYE9g3L(al}cE*G!p|I>!--&Gfte_Epo(E z@!6+-PrmVKb+ zmr1hhcaHyKdG4;&x##LHZKp<`kd5DDQ8}I^&9XPWW4%1*)#}{*{K8Q_&sp^_xU=lI z6^jS-6U*u~pX=g?b-7ZOJ-TF?JRjBSEZwqaJ0qlrWrSLPWAA2r0*%ifD_VbbW}SSW z9kTw;OqTuViQ3Ndm!C3v(dL!;)61&iuiWq#J+y+Y&Z=26hE~m5eO6+Qt+3m&e6h#by2n~JbN?Eq#lW;tE;djRF_a|~y z6HnywSxX?K=1;voMV?+xcRy4U&n5DS!;tm2g_2kAocGGklea8uQo~=P;IC99#EBbD z=qz3P!6r5Q^$X2H{8mSn-T&hC_QbDZsR>zoiQXoT$>LDo$(>~{O+E7EKu{ALTD3CA z>iXY}-gn2*ZCVfh4U}dfe&0!D&${R8$hG!_wB;%^>=M*N5KD*NHg?>5=3C9V7Olvw zk+(WWPyUwuok8_k)OevE-nqjBlMZH$BHDk9}cv5JV_MNwJnp>c#9 nUKRJ+sZlxRmC71ljkU<;t! literal 0 HcmV?d00001 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]