From 71170d638455b34ddbea6ef12946aa6e6fb9f43b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 20 Jun 2017 20:22:14 -0700 Subject: [PATCH 1/2] 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] From 9213f944f19080908acb3e1ac3609acebbd68ce1 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 21 Jun 2017 09:33:46 -0700 Subject: [PATCH 2/2] add kukaCamGymEnv.py with camera observations (preliminary) show camera position in example browser disable per-vertex and per-fragment profile timings --- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 4 +- examples/TinyRenderer/TinyRenderer.cpp | 11 +- examples/pybullet/gym/envs/__init__.py | 7 + examples/pybullet/gym/envs/bullet/__init__.py | 1 + .../pybullet/gym/envs/bullet/kukaCamGymEnv.py | 191 ++++++++++++++++++ .../pybullet/gym/train_kuka_cam_grasping.py | 44 ++++ .../pybullet/gym/train_pybullet_cartpole.py | 33 +++ 7 files changed, 286 insertions(+), 5 deletions(-) create mode 100644 examples/pybullet/gym/envs/bullet/kukaCamGymEnv.py create mode 100644 examples/pybullet/gym/train_kuka_cam_grasping.py create mode 100644 examples/pybullet/gym/train_pybullet_cartpole.py diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index b4e7ea3d6..7459729af 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -1296,8 +1296,10 @@ void OpenGLExampleBrowser::update(float deltaTime) float pitch = s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraPitch(); float yaw = s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraYaw(); float camTarget[3]; + float camPos[3]; + s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraPosition(camPos); s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraTargetPosition(camTarget); - sprintf(msg,"dist=%f, pitch=%f, yaw=%f,target=%f,%f,%f", camDist,pitch,yaw,camTarget[0],camTarget[1],camTarget[2]); + sprintf(msg,"camPos=%f,%f,%f, dist=%f, pitch=%f, yaw=%f,target=%f,%f,%f", camPos[0],camPos[1],camPos[2],camDist,pitch,yaw,camTarget[0],camTarget[1],camTarget[2]); gui2->setStatusBarMessage(msg, true); } diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 23f9fda10..a2ea07307 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -124,7 +124,7 @@ struct Shader : public IShader { m_projectionLightViewMat = m_projectionMat*m_lightModelView; } virtual Vec4f vertex(int iface, int nthvert) { - B3_PROFILE("vertex"); + //B3_PROFILE("vertex"); Vec2f uv = m_model->uv(iface, nthvert); varying_uv.set_col(nthvert, uv); varying_nrm.set_col(nthvert, proj<3>(m_invModelMat*embed<4>(m_model->normal(iface, nthvert), 0.f))); @@ -142,7 +142,7 @@ struct Shader : public IShader { } virtual bool fragment(Vec3f bar, TGAColor &color) { - B3_PROFILE("fragment"); + //B3_PROFILE("fragment"); Vec4f p = m_viewportMat*(varying_tri_light_view*bar); float depth = p[2]; p = p/p[3]; @@ -485,6 +485,7 @@ static bool clipTriangleAgainstNearplane(const mat<4,3,float>& triangleIn, b3Ali void TinyRenderer::renderObject(TinyRenderObjectData& renderData) { + B3_PROFILE("renderObject"); int width = renderData.m_rgbColorBuffer.get_width(); int height = renderData.m_rgbColorBuffer.get_height(); @@ -517,10 +518,11 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) Shader shader(model, light_dir_local, light_color, modelViewMatrix, lightModelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, renderData.m_viewportMatrix, localScaling, model->getColorRGBA(), width, height, shadowBufferPtr, renderData.m_lightAmbientCoeff, renderData.m_lightDiffuseCoeff, renderData.m_lightSpecularCoeff); - + { + B3_PROFILE("face"); + for (int i=0; infaces(); i++) { - B3_PROFILE("face"); for (int j=0; j<3; j++) { shader.vertex(i, j); } @@ -552,6 +554,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex); } } + } } } diff --git a/examples/pybullet/gym/envs/__init__.py b/examples/pybullet/gym/envs/__init__.py index d8b096113..ae0c1c81f 100644 --- a/examples/pybullet/gym/envs/__init__.py +++ b/examples/pybullet/gym/envs/__init__.py @@ -42,4 +42,11 @@ register( entry_point='envs.bullet:KukaGymEnv', timestep_limit=1000, reward_threshold=5.0, +) + +register( + id='KukaCamBulletEnv-v0', + entry_point='envs.bullet:KukaCamGymEnv', + timestep_limit=1000, + reward_threshold=5.0, ) \ No newline at end of file diff --git a/examples/pybullet/gym/envs/bullet/__init__.py b/examples/pybullet/gym/envs/bullet/__init__.py index 708ae4e53..f86a691b1 100644 --- a/examples/pybullet/gym/envs/bullet/__init__.py +++ b/examples/pybullet/gym/envs/bullet/__init__.py @@ -4,3 +4,4 @@ from envs.bullet.racecarGymEnv import RacecarGymEnv from envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv from envs.bullet.humanoidGymEnv import HumanoidGymEnv from envs.bullet.kukaGymEnv import KukaGymEnv +from envs.bullet.kukaCamGymEnv import KukaCamGymEnv \ No newline at end of file diff --git a/examples/pybullet/gym/envs/bullet/kukaCamGymEnv.py b/examples/pybullet/gym/envs/bullet/kukaCamGymEnv.py new file mode 100644 index 000000000..2af6774cd --- /dev/null +++ b/examples/pybullet/gym/envs/bullet/kukaCamGymEnv.py @@ -0,0 +1,191 @@ +import math +import gym +from gym import spaces +from gym.utils import seeding +import numpy as np +import time +import pybullet as p +from . import kuka +import random + +class KukaCamGymEnv(gym.Env): + metadata = { + 'render.modes': ['human', 'rgb_array'], + 'video.frames_per_second' : 50 + } + + def __init__(self, + urdfRoot="", + actionRepeat=1, + isEnableSelfCollision=True, + renders=True): + print("init") + self._timeStep = 1./240. + self._urdfRoot = urdfRoot + self._actionRepeat = actionRepeat + self._isEnableSelfCollision = isEnableSelfCollision + self._observation = [] + self._envStepCounter = 0 + self._renders = renders + self._width = 341 + self._height = 256 + self.terminated = 0 + 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") + self._seed() + self.reset() + observationDim = len(self.getExtendedObservation()) + #print("observationDim") + #print(observationDim) + + observation_high = np.array([np.finfo(np.float32).max] * observationDim) + self.action_space = spaces.Discrete(7) + self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4)) + self.viewer = None + + def _reset(self): + print("reset") + self.terminated = 0 + p.resetSimulation() + p.setPhysicsEngineParameter(numSolverIterations=150) + p.setTimeStep(self._timeStep) + p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1]) + + 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() + ypos = 0 +0.05*random.random() + ang = 3.1415925438*random.random() + orn = p.getQuaternionFromEuler([0,0,ang]) + self.blockUid =p.loadURDF("block.urdf", xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3]) + + p.setGravity(0,0,-10) + self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) + self._envStepCounter = 0 + p.stepSimulation() + self._observation = self.getExtendedObservation() + return np.array(self._observation) + + def __del__(self): + p.disconnect() + + def _seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def getExtendedObservation(self): + + #camEyePos = [0.03,0.236,0.54] + #distance = 1.06 + #pitch=-56 + #yaw = 258 + #roll=0 + #upAxisIndex = 2 + #camInfo = p.getDebugVisualizerCamera() + #print("width,height") + #print(camInfo[0]) + #print(camInfo[1]) + #print("viewMatrix") + #print(camInfo[2]) + #print("projectionMatrix") + #print(camInfo[3]) + #viewMat = camInfo[2] + #viewMat = p.computeViewMatrixFromYawPitchRoll(camEyePos,distance,yaw, pitch,roll,upAxisIndex) + viewMat = [-0.5120397806167603, 0.7171027660369873, -0.47284144163131714, 0.0, -0.8589617609977722, -0.42747554183006287, 0.28186774253845215, 0.0, 0.0, 0.5504802465438843, 0.8348482847213745, 0.0, 0.1925382763147354, -0.24935829639434814, -0.4401884973049164, 1.0] + #projMatrix = camInfo[3]#[0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] + projMatrix = [0.75, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] + + img_arr = p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix) + rgb=img_arr[2] + np_img_arr = np.reshape(rgb, (self._height, self._width, 4)) + self._observation = np_img_arr + return self._observation + + def _step(self, action): + 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] + f = 0.3 + realAction = [dx,dy,-0.002,da,f] + return self.step2( realAction) + + def step2(self, action): + self._kuka.applyAction(action) + for i in range(self._actionRepeat): + p.stepSimulation() + if self._renders: + time.sleep(self._timeStep) + self._observation = self.getExtendedObservation() + if self._termination(): + break + self._envStepCounter += 1 + #print("self._envStepCounter") + #print(self._envStepCounter) + + done = self._termination() + reward = self._reward() + #print("len=%r" % len(self._observation)) + + return np.array(self._observation), reward, done, {} + + def _render(self, mode='human', close=False): + return + + def _termination(self): + #print (self._kuka.endEffectorPos[2]) + state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) + actualEndEffectorPos = state[0] + + #print("self._envStepCounter") + #print(self._envStepCounter) + if (self.terminated or self._envStepCounter>1000): + self._observation = self.getExtendedObservation() + return True + + if (actualEndEffectorPos[2] <= 0.10): + self.terminated = 1 + + #print("closing gripper, attempting grasp") + #start grasp and terminate + fingerAngle = 0.3 + + for i in range (1000): + graspAction = [0,0,0.001,0,fingerAngle] + self._kuka.applyAction(graspAction) + p.stepSimulation() + fingerAngle = fingerAngle-(0.3/100.) + if (fingerAngle<0): + fingerAngle=0 + + self._observation = self.getExtendedObservation() + return True + return False + + def _reward(self): + + #rewards is height of target object + blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) + closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000) + + reward = -1000 + numPt = len(closestPoints) + #print(numPt) + if (numPt>0): + #print("reward:") + reward = -closestPoints[0][8]*10 + + if (blockPos[2] >0.2): + print("grasped a block!!!") + print("self._envStepCounter") + print(self._envStepCounter) + reward = reward+1000 + + #print("reward") + #print(reward) + return reward diff --git a/examples/pybullet/gym/train_kuka_cam_grasping.py b/examples/pybullet/gym/train_kuka_cam_grasping.py new file mode 100644 index 000000000..93d4a2b78 --- /dev/null +++ b/examples/pybullet/gym/train_kuka_cam_grasping.py @@ -0,0 +1,44 @@ +import gym +from envs.bullet.kukaCamGymEnv import KukaCamGymEnv + +from baselines import deepq + +import datetime + + + +def callback(lcl, glb): + # stop training if reward exceeds 199 + total = sum(lcl['episode_rewards'][-101:-1]) / 100 + totalt = lcl['t'] + #print("totalt") + #print(totalt) + is_solved = totalt > 2000 and total >= 10 + return is_solved + + +def main(): + + env = KukaCamGymEnv(renders=True) + model = deepq.models.cnn_to_mlp( + convs=[(32, 8, 4), (64, 4, 2), (64, 3, 1)], + hiddens=[256], + dueling=False + ) + act = deepq.learn( + env, + q_func=model, + lr=1e-3, + max_timesteps=10000000, + buffer_size=50000, + exploration_fraction=0.1, + exploration_final_eps=0.02, + print_freq=10, + callback=callback + ) + print("Saving model to kuka_cam_model.pkl") + act.save("kuka_cam_model.pkl") + + +if __name__ == '__main__': + main() diff --git a/examples/pybullet/gym/train_pybullet_cartpole.py b/examples/pybullet/gym/train_pybullet_cartpole.py new file mode 100644 index 000000000..353fa4a86 --- /dev/null +++ b/examples/pybullet/gym/train_pybullet_cartpole.py @@ -0,0 +1,33 @@ +import gym +from envs.bullet.cartpole_bullet import CartPoleBulletEnv + +from baselines import deepq + + +def callback(lcl, glb): + # stop training if reward exceeds 199 + is_solved = lcl['t'] > 100 and sum(lcl['episode_rewards'][-101:-1]) / 100 >= 199 + return is_solved + + +def main(): + + env = gym.make('CartPoleBulletEnv-v0') + model = deepq.models.mlp([64]) + act = deepq.learn( + env, + q_func=model, + lr=1e-3, + max_timesteps=100000, + buffer_size=50000, + exploration_fraction=0.1, + exploration_final_eps=0.02, + print_freq=10, + callback=callback + ) + print("Saving model to cartpole_model.pkl") + act.save("cartpole_model.pkl") + + +if __name__ == '__main__': + main()