diff --git a/Extras/Serialize/CMakeLists.txt b/Extras/Serialize/CMakeLists.txt index 68b1d1925..bc7ddde17 100644 --- a/Extras/Serialize/CMakeLists.txt +++ b/Extras/Serialize/CMakeLists.txt @@ -1,6 +1,6 @@ IF (BUILD_BLEND_DEMO OR INTERNAL_UPDATE_SERIALIZATION_STRUCTURES) - SUBDIRS(BlenderSerialize ) + #SUBDIRS(BlenderSerialize ) ENDIF() diff --git a/Extras/Serialize/HeaderGenerator/CMakeLists.txt b/Extras/Serialize/HeaderGenerator/CMakeLists.txt index 4739fcbdf..3c1e862c8 100644 --- a/Extras/Serialize/HeaderGenerator/CMakeLists.txt +++ b/Extras/Serialize/HeaderGenerator/CMakeLists.txt @@ -13,7 +13,7 @@ SET(includes LINK_LIBRARIES( - BulletFileLoader BlenderSerialize LinearMath + BulletFileLoader LinearMath ) INCLUDE_DIRECTORIES(${includes}) diff --git a/Extras/Serialize/makesdna/makesdna.cpp b/Extras/Serialize/makesdna/makesdna.cpp index 77deaa63e..72795506e 100644 --- a/Extras/Serialize/makesdna/makesdna.cpp +++ b/Extras/Serialize/makesdna/makesdna.cpp @@ -86,10 +86,12 @@ typedef unsigned __int64 uint64_t; /* Linux-i386, Linux-Alpha, Linux-ppc */ #include +typedef intptr_t btintptr_t; #elif defined (__APPLE__) #include +typedef intptr_t btintptr_t; #elif defined(FREE_WINDOWS) diff --git a/data/wood.jpg b/data/wood.jpg new file mode 100644 index 000000000..fab97ab13 Binary files /dev/null and b/data/wood.jpg differ diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 25bae7c86..1db703604 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -2,6 +2,9 @@ #include "LinearMath/btTransform.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" + + #include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" @@ -502,6 +505,15 @@ void ConvertURDF2BulletInternal( col->setCollisionShape(compoundShape); + if (compoundShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)compoundShape; + if (trimeshShape->getTriangleInfoMap()) + { + col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + } + } + btTransform tr; tr.setIdentity(); tr = linkTransformInWorldSpace; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 0ee22969b..2a6009920 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -566,6 +566,14 @@ B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMem return 0; } +B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryCommandHandle commandHandle, double allowedCcdPenetration) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_allowedCcdPenetration = allowedCcdPenetration; + command->m_updateFlags |= SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION ; + return 0; +} B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) { @@ -2308,6 +2316,17 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHan return 0; } +B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex; + command->m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius = ccdSweptSphereRadius; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS; + return 0; +} + B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 6dea73129..7b0799de8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -125,6 +125,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHand B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping); B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping); B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor); +B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius); B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); @@ -291,9 +292,7 @@ B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold); B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle, int enableConeFriction); B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs); - - - +B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryCommandHandle commandHandle, double allowedCcdPenetration); B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index a0471ff1d..049399eaf 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6,6 +6,8 @@ #include "../Importers/ImportURDFDemo/URDF2Bullet.h" #include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp" +#include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h" + #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" @@ -1727,6 +1729,7 @@ void logCallback(btDynamicsWorld *world, btScalar timeStep) bool MyContactAddedCallback(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) { + btAdjustInternalEdgeContacts(cp, colObj1Wrap, colObj0Wrap, partId1,index1); return true; } @@ -2253,12 +2256,13 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() isPreTick = true; m_data->m_dynamicsWorld->setInternalTickCallback(preTickCallback,this,isPreTick); + gContactAddedCallback = MyContactAddedCallback; #ifdef B3_ENABLE_TINY_AUDIO m_data->m_soundEngine.init(16,true); //we don't use those callbacks (yet), experimental -// gContactAddedCallback = MyContactAddedCallback; + // gContactDestroyedCallback = MyContactDestroyedCallback; // gContactProcessedCallback = MyContactProcessedCallback; // gContactStartedCallback = MyContactStartedCallback; @@ -2411,6 +2415,17 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() for (int j = 0; jm_collisionShapes.size(); j++) { btCollisionShape* shape = m_data->m_collisionShapes[j]; + + + //check for internal edge utility, delete memory + if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) shape; + if (trimesh->getTriangleInfoMap()) + { + delete trimesh->getTriangleInfoMap(); + } + } delete shape; } for (int j=0;jm_meshInterfaces.size();j++) @@ -3823,10 +3838,17 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str meshInterface->addTriangle(v0, v1, v2); } } + { BT_PROFILE("create btBvhTriangleMeshShape"); btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true); m_data->m_collisionShapes.push_back(trimesh); + + if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_CONCAVE_INTERNAL_EDGE) + { + btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap(); + btGenerateInternalEdgeInfo(trimesh, triangleInfoMap); + } //trimesh->setLocalScaling(collision->m_geometry.m_meshScale); shape = trimesh; if (compound) @@ -6377,6 +6399,13 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc body->m_rigidBody->setMassProps(mass,newLocalInertiaDiagonal); } } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS) + { + body->m_rigidBody->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius); + //for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled + body->m_rigidBody->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius/2.); + } } } @@ -6534,6 +6563,12 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st { m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs = (clientCmd.m_physSimParamArgs.m_deterministicOverlappingPairs!=0); } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION) + { + m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration = clientCmd.m_physSimParamArgs.m_allowedCcdPenetration; + } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) { m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 99089ce7e..bdbf64f60 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -159,7 +159,7 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING=256, CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR = 512, CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL = 1024, - + CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS = 2048, }; struct ChangeDynamicsInfoArgs @@ -178,6 +178,7 @@ struct ChangeDynamicsInfoArgs double m_contactDamping; double m_localInertiaDiagonal[3]; int m_frictionAnchor; + double m_ccdSweptSphereRadius; }; struct GetDynamicsInfoArgs @@ -438,6 +439,7 @@ enum EnumSimParamUpdateFlags SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP=16384, SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768, SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 65536, + SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION = 131072, }; enum EnumLoadSoftBodyUpdateFlags diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index fa1a31201..ee601ac4e 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -21,15 +21,15 @@ public: InProcessPhysicsClientSharedMemoryMainThread(int argc, char* argv[], bool useInProcessMemory) { - int newargc = argc+2; + int newargc = argc+3; char** newargv = (char**)malloc(sizeof(void*)*newargc); - for (int i=0;i=0): + self._pybullet_client.restoreState(self.stateId) + index=0 + for i in self._cubes: + posX = random.uniform(-xPosRange,xPosRange) + posY = random.uniform(-yPosRange,yPosRange) + yaw = random.uniform(-math.pi,math.pi) + pos=[posX,posY,boxHalfExtents + index*2*boxHalfExtents] + index+=1 + orn = self._pybullet_client.getQuaternionFromEuler([0,0,yaw]) + self._pybullet_client.resetBasePositionAndOrientation(i,pos,orn) + + def GetActionDimension(self): + """Get the length of the action list. + + Returns: + The length of the action list. + """ + return 4#self.num_motors + + def GetObservationUpperBound(self): + """Get the upper bound of the observation. + + Returns: + The upper bound of an observation. See GetObservation() for the details + of each element of an observation. + """ + upper_bound = np.array([0.0] * self.GetObservationDimension()) + upper_bound[0:] = 1.0 # Quaternion of base orientation. + return upper_bound + + def GetObservationLowerBound(self): + """Get the lower bound of the observation.""" + return -self.GetObservationUpperBound() + + def GetObservationDimension(self): + """Get the length of the observation list. + + Returns: + The length of the observation list. + """ + sz = len(self.GetObservation()) + print("sz=",sz) + return sz + + def GetObservation(self): + """Get the observations of minitaur. + + Returns: + The observation list. observation[0:4] is the base orientation in quaternion form. + """ + observation = [] + for i in self._cubes: + pos,orn=self._pybullet_client.getBasePositionAndOrientation(i) + observation.extend(list(pos)) + observation.extend(list(orn)) + return observation + + def ApplyAction(self, action): + """Set the desired action. + """ + self.m_actions_taken_since_reset+=1 + + + def Termination(self): + return self.m_actions_taken_since_reset>=200 + +def CreateSim(pybullet_client,urdf_root,time_step): + sim = BoxStackPyBulletSim(pybullet_client=pybullet_client, + urdf_root=urdf_root, + time_step=time_step) + return sim diff --git a/examples/pybullet/gym/pybullet_envs/prediction/pybullet_sim_gym_env.py b/examples/pybullet/gym/pybullet_envs/prediction/pybullet_sim_gym_env.py new file mode 100644 index 000000000..5c1b52757 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/prediction/pybullet_sim_gym_env.py @@ -0,0 +1,219 @@ +"""This file implements the gym environment of example PyBullet simulation. + +""" + +import os, inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0,parentdir) + + +import math +import time +import gym +from gym import spaces +from gym.utils import seeding +import numpy as np +import pybullet +from pybullet_envs.bullet import bullet_client + +from pybullet_envs.prediction import boxstack_pybullet_sim + +import os +import pybullet_data + +from pkg_resources import parse_version + + +class PyBulletSimGymEnv(gym.Env): + """The gym environment to run pybullet simulations. + + + """ + metadata = { + "render.modes": ["human", "rgb_array"], + "video.frames_per_second": 50 + } + + def __init__(self,pybullet_sim_factory = boxstack_pybullet_sim, + render=True, + render_sleep=False, + debug_visualization=True, + hard_reset = False, + render_width=240, + render_height=240, + action_repeat=1, + time_step = 1./240., + num_bullet_solver_iterations=50, + urdf_root=pybullet_data.getDataPath()): + """Initialize the gym environment. + + Args: + urdf_root: The path to the urdf data folder. + """ + self._pybullet_sim_factory = pybullet_sim_factory + self._time_step = time_step + self._urdf_root = urdf_root + self._observation = [] + self._action_repeat=action_repeat + self._num_bullet_solver_iterations = num_bullet_solver_iterations + self._env_step_counter = 0 + self._is_render = render + self._debug_visualization = debug_visualization + self._render_sleep = render_sleep + self._render_width=render_width + self._render_height=render_height + self._cam_dist = .3 + self._cam_yaw = 50 + self._cam_pitch = -35 + self._hard_reset = True + self._last_frame_time = 0.0 + + optionstring='--width={} --height={}'.format(render_width,render_height) + + print("urdf_root=" + self._urdf_root) + + if self._is_render: + self._pybullet_client = bullet_client.BulletClient( + connection_mode=pybullet.GUI, options=optionstring) + else: + self._pybullet_client = bullet_client.BulletClient() + + if (debug_visualization==False): + self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_GUI,enable=0) + self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_RGB_BUFFER_PREVIEW,enable=0) + self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_DEPTH_BUFFER_PREVIEW,enable=0) + self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,enable=0) + + + self._pybullet_client.setAdditionalSearchPath(urdf_root) + self._seed() + self.reset() + + observation_high = ( + self._example_sim.GetObservationUpperBound()) + observation_low = ( + self._example_sim.GetObservationLowerBound()) + + action_dim = self._example_sim.GetActionDimension() + self._action_bound = 1 + action_high = np.array([self._action_bound] * action_dim) + self.action_space = spaces.Box(-action_high, action_high) + self.observation_space = spaces.Box(observation_low, observation_high) + + self.viewer = None + self._hard_reset = hard_reset # This assignment need to be after reset() + + + def configure(self, args): + self._args = args + + def _reset(self): + if self._hard_reset: + self._pybullet_client.resetSimulation() + + self._pybullet_client.setPhysicsEngineParameter( + numSolverIterations=int(self._num_bullet_solver_iterations)) + self._pybullet_client.setTimeStep(self._time_step) + + self._example_sim = self._pybullet_sim_factory.CreateSim( + pybullet_client=self._pybullet_client, + urdf_root=self._urdf_root, + time_step=self._time_step) + else: + self._example_sim.Reset(reload_urdf=False) + + self._env_step_counter = 0 + + #self._pybullet_client.resetDebugVisualizerCamera( + # self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0]) + + return self._get_observation() + + def _seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def _step(self, action): + """Step forward the simulation, given the action. + + Args: + action: the predicted state + + Returns: + observations: The actual state. + reward: The reward for how well the prediction matches the actual state. + done: Whether the episode has ended. + info: A dictionary that stores diagnostic information. + + Raises: + ValueError: The action dimension is not the same as the number of motors. + ValueError: The magnitude of actions is out of bounds. + """ + if self._render_sleep: + # Sleep, otherwise the computation takes less time than real time, + # which will make the visualization like a fast-forward video. + time_spent = time.time() - self._last_frame_time + self._last_frame_time = time.time() + time_to_sleep = self._action_repeat * self._time_step - time_spent + if time_to_sleep > 0: + time.sleep(time_to_sleep) + + #base_pos = self.minitaur.GetBasePosition() + #self._pybullet_client.resetDebugVisualizerCamera( + # self._cam_dist, self._cam_yaw, self._cam_pitch, base_pos) + + + for _ in range(self._action_repeat): + self._example_sim.ApplyAction(action) + self._pybullet_client.stepSimulation() + + self._env_step_counter += 1 + reward = self._reward() + done = self._termination() + return np.array(self._get_observation()), reward, done, {} + + def _render(self, mode="rgb_array", close=False): + if mode != "rgb_array": + return np.array([]) + base_pos = [0,0,0] + view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll( + cameraTargetPosition=base_pos, + distance=self._cam_dist, + yaw=self._cam_yaw, + pitch=self._cam_pitch, + roll=0, + upAxisIndex=2) + proj_matrix = self._pybullet_client.computeProjectionMatrixFOV( + fov=60, aspect=float(self._render_width)/self._render_width, + nearVal=0.01, farVal=100.0) + proj_matrix=[1.0, 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] + (_, _, px, _, _) = self._pybullet_client.getCameraImage( + width=self._render_width, height=self._render_height, viewMatrix=view_matrix, + projectionMatrix=proj_matrix, renderer=pybullet.ER_TINY_RENDERER) + rgb_array = np.array(px, dtype=np.uint8) + rgb_array = np.reshape(rgb_array, (self._render_height, self._render_width, 4)) + rgb_array = rgb_array[:, :, :3] + return rgb_array + + + + def _termination(self): + terminate=self._example_sim.Termination() + return terminate + + def _reward(self): + reward = 0 + return reward + + + def _get_observation(self): + self._observation = self._example_sim.GetObservation() + return self._observation + + + if parse_version(gym.__version__)>=parse_version('0.9.6'): + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/prediction/test_pybullet_sim_gym_env.py b/examples/pybullet/gym/pybullet_envs/prediction/test_pybullet_sim_gym_env.py new file mode 100644 index 000000000..23f734e4e --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/prediction/test_pybullet_sim_gym_env.py @@ -0,0 +1,60 @@ +r"""An example to run of the minitaur gym environment with sine gaits. +""" + +import os +import inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0,parentdir) + +import math +import numpy as np +from pybullet_envs.prediction import pybullet_sim_gym_env +import argparse +import time + +from pybullet_envs.prediction import boxstack_pybullet_sim +from gym.wrappers.monitoring import video_recorder + +def ResetPoseExample(steps): + """An example that the minitaur stands still using the reset pose.""" + + + environment = pybullet_sim_gym_env.PyBulletSimGymEnv( + pybullet_sim_factory=boxstack_pybullet_sim, + debug_visualization=False, + render=True, action_repeat=30) + action = [math.pi / 2] * 8 + + vid = video_recorder.VideoRecorder(env=environment,path="vid.mp4") + + for _ in range(steps): + print(_) + startsim = time.time() + _, _, done, _ = environment.step(action) + stopsim = time.time() + startrender = time.time() + #environment.render(mode='rgb_array') + vid.capture_frame() + stoprender = time.time() + print ("env.step " , (stopsim - startsim)) + print ("env.render " , (stoprender - startrender)) + if done: + environment.reset() + + + + +def main(): + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument('--env', help='environment ID (0==reset)',type=int, default=0) + args = parser.parse_args() + print("--env=" + str(args.env)) + + if (args.env == 0): + ResetPoseExample(steps = 1000) + +if __name__ == '__main__': + main() + + diff --git a/examples/pybullet/notebooks/HelloPyBullet.ipynb b/examples/pybullet/notebooks/HelloPyBullet.ipynb new file mode 100644 index 000000000..e31775fe7 --- /dev/null +++ b/examples/pybullet/notebooks/HelloPyBullet.ipynb @@ -0,0 +1,254 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "collapsed": true + }, + "outputs": [], + "source": [ + "import pybullet as p" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "0" + ] + }, + "execution_count": 2, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "p.connect(p.DIRECT)" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "0" + ] + }, + "execution_count": 3, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "p.loadURDF(\"plane.urdf\")" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "r2d2=p.loadURDF(\"r2d2.urdf\",[0,0,0.5])" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "2" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "p.getNumBodies()" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "joint 0 name= base_to_right_leg\n", + "joint 1 name= right_base_joint\n", + "joint 2 name= right_front_wheel_joint\n", + "joint 3 name= right_back_wheel_joint\n", + "joint 4 name= base_to_left_leg\n", + "joint 5 name= left_base_joint\n", + "joint 6 name= left_front_wheel_joint\n", + "joint 7 name= left_back_wheel_joint\n", + "joint 8 name= gripper_extension\n", + "joint 9 name= left_gripper_joint\n", + "joint 10 name= left_tip_joint\n", + "joint 11 name= right_gripper_joint\n", + "joint 12 name= right_tip_joint\n", + "joint 13 name= head_swivel\n", + "joint 14 name= tobox\n" + ] + } + ], + "source": [ + "for i in range (p.getNumJoints(r2d2)):\n", + " jointInfo=p.getJointInfo(r2d2,i)\n", + " print(\"joint\",jointInfo[0],\"name=\",jointInfo[1].decode('ascii'))" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "pos = 0.00000,0.00000,0.50000 \n", + "pos = 0.00000,-0.00000,0.49983 \n", + "pos = 0.00000,-0.00000,0.49948 \n", + "pos = 0.00000,-0.00000,0.49896 \n", + "pos = 0.00000,-0.00000,0.49826 \n", + "pos = 0.00000,-0.00000,0.49740 \n", + "pos = 0.00000,-0.00000,0.49636 \n", + "pos = 0.00000,-0.00000,0.49514 \n", + "pos = 0.00000,-0.00000,0.49375 \n", + "pos = 0.00000,-0.00000,0.49219 \n" + ] + } + ], + "source": [ + "p.setGravity(0,0,-10)\n", + "precision=5\n", + "for i in range (10):\n", + " pos,orn = p.getBasePositionAndOrientation(r2d2)\n", + " posmsg='pos = {posx:.{prec}f},{posy:.{prec}f},{posz:.{prec}f} '.format(posx=pos[0],posy=pos[1],posz=pos[2], prec=precision)\n", + " print(posmsg)\n", + " p.stepSimulation()" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [], + "source": [ + "p.stepSimulation()" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": { + "collapsed": true + }, + "outputs": [], + "source": [ + "%matplotlib inline\n", + "\n", + "import matplotlib\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": {}, + "outputs": [], + "source": [ + "camTargetPos = [0,0,0]\n", + "cameraUp = [0,0,1]\n", + "cameraPos = [1,1,1]\n", + "\n", + "pitch = -10.0\n", + "yaw = 60\n", + "roll=0\n", + "upAxisIndex = 2\n", + "camDistance = 4\n", + "pixelWidth = 320\n", + "pixelHeight = 200\n", + "nearPlane = 0.01\n", + "farPlane = 100\n", + "fov = 60\n", + "viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)\n", + "aspect = pixelWidth / pixelHeight;\n", + "projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);\n", + "img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, shadow=1,lightDirection=[1,1,1])\n" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "iVBORw0KGgoAAAANSUhEUgAAAXoAAAD0CAYAAACVbe2MAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\n", + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "w=img_arr[0] #width of the image, in pixels\n", + "h=img_arr[1] #height of the image, in pixels\n", + "rgb=img_arr[2] #color data RGB\n", + "plt.imshow(rgb,interpolation='none')\n", + "plt.draw()\n", + "plt.show()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "collapsed": true + }, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.6.1" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f2e8dd194..0fd41f4bd 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -382,6 +382,11 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P #endif break; } + case eCONNECT_GUI_MAIN_THREAD: + { + sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); + break; + } case eCONNECT_GUI_SERVER: { @@ -870,14 +875,15 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO double angularDamping = -1; double contactStiffness = -1; double contactDamping = -1; + double ccdSweptSphereRadius=-1; int frictionAnchor = -1; PyObject* localInertiaDiagonalObj=0; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOdi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &physicsClientId)) { return NULL; } @@ -937,6 +943,10 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO { b3ChangeDynamicsInfoSetFrictionAnchor(command,bodyUniqueId,linkIndex, frictionAnchor); } + if (ccdSweptSphereRadius>=0) + { + b3ChangeDynamicsInfoSetCcdSweptSphereRadius(command,bodyUniqueId,linkIndex, ccdSweptSphereRadius); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } @@ -1099,15 +1109,17 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar double erp = -1; double contactERP = -1; double frictionERP = -1; + double allowedCcdPenetration = -1; + int enableConeFriction = -1; b3PhysicsClientHandle sm = 0; int deterministicOverlappingPairs = -1; int physicsClientId = 0; - static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "physicsClientId", NULL}; + static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, - &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &physicsClientId)) { return NULL; } @@ -1188,6 +1200,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar b3PhysicsParameterSetDeterministicOverlappingPairs(command,deterministicOverlappingPairs); } + if (allowedCcdPenetration>=0) + { + b3PhysicsParameterSetAllowedCcdPenetration(command,allowedCcdPenetration); + + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } @@ -8776,6 +8794,7 @@ initpybullet(void) PyModule_AddIntConstant(m, "UDP", eCONNECT_UDP); // user read PyModule_AddIntConstant(m, "TCP", eCONNECT_TCP); // user read PyModule_AddIntConstant(m, "GUI_SERVER", eCONNECT_GUI_SERVER); // user read + PyModule_AddIntConstant(m, "GUI_MAIN_THREAD", eCONNECT_GUI_MAIN_THREAD); // user read PyModule_AddIntConstant(m, "JOINT_REVOLUTE", eRevoluteType); // user read PyModule_AddIntConstant(m, "JOINT_PRISMATIC", ePrismaticType); // user read @@ -8902,6 +8921,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE); PyModule_AddIntConstant(m, "GEOM_FORCE_CONCAVE_TRIMESH", GEOM_FORCE_CONCAVE_TRIMESH); + PyModule_AddIntConstant(m, "GEOM_CONCAVE_INTERNAL_EDGE", GEOM_CONCAVE_INTERNAL_EDGE); + PyModule_AddIntConstant(m, "STATE_LOG_JOINT_MOTOR_TORQUES", STATE_LOG_JOINT_MOTOR_TORQUES); PyModule_AddIntConstant(m, "STATE_LOG_JOINT_USER_TORQUES", STATE_LOG_JOINT_USER_TORQUES);