diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 797aa86ea..2ce391c60 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -37,6 +37,8 @@ public: virtual int getNumUserConstraints() const = 0; virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const = 0; + + virtual int getUserConstraintId(int serialIndex) const = 0; virtual void setSharedMemoryKey(int key) = 0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 577e70e56..150c6a242 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1179,6 +1179,13 @@ int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniq return 0; } +/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() ) +int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + return cl->getUserConstraintId(serialIndex); +} + /// return the body unique id, given the index in range [0 , b3GetNumBodies() ) int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex) { @@ -1208,7 +1215,41 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd return cl->getJointInfo(bodyIndex, jointIndex, *info); } +b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_RESET_DYNAMIC_INFO; + command->m_updateFlags = 0; + + return (b3SharedMemoryCommandHandle) command; +} +int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_RESET_DYNAMIC_INFO); + b3Assert(mass > 0); + command->m_resetDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_resetDynamicInfoArgs.m_linkIndex = linkIndex; + command->m_resetDynamicInfoArgs.m_mass = mass; + command->m_updateFlags |= RESET_DYNAMIC_INFO_SET_MASS; + return 0; +} + +int b3ResetDynamicInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_RESET_DYNAMIC_INFO); + command->m_resetDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_resetDynamicInfoArgs.m_linkIndex = linkIndex; + command->m_resetDynamicInfoArgs.m_lateralFriction = lateralFriction; + command->m_updateFlags |= RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION; + return 0; +} 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 49f26d11a..61db28054 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -76,6 +76,13 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); ///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info); + +///given a body unique id and link index, return the dynamic information. See b3DynamicInfo in SharedMemoryPublic.h +int b3GetDynamicInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, struct b3DynamicInfo* info); + +b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient); +int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass); +int b3ResetDynamicInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction); b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); @@ -92,6 +99,8 @@ b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHa int b3GetNumUserConstraints(b3PhysicsClientHandle physClient); int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info); +/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() ) +int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex); ///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index c64eb338e..29df1e06e 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -163,6 +163,15 @@ int PhysicsClientSharedMemory::getUserConstraintInfo(int constraintUniqueId, str return 0; } +int PhysicsClientSharedMemory::getUserConstraintId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints())) + { + return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + PhysicsClientSharedMemory::PhysicsClientSharedMemory() { diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index c74ca9182..b436a9fef 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -48,6 +48,8 @@ public: virtual int getNumUserConstraints() const; virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const; + + virtual int getUserConstraintId(int serialIndex) const; virtual void setSharedMemoryKey(int key); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 234b7ff77..e3384d871 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -961,7 +961,14 @@ int PhysicsDirect::getUserConstraintInfo(int constraintUniqueId, struct b3UserCo return 0; } - +int PhysicsDirect::getUserConstraintId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints())) + { + return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} int PhysicsDirect::getBodyUniqueId(int serialIndex) const { diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index d3f71168a..64ea073eb 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -69,6 +69,8 @@ public: virtual int getNumUserConstraints() const; virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const; + + virtual int getUserConstraintId(int serialIndex) const; ///todo: move this out of the virtual void setSharedMemoryKey(int key); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 69236d71f..bf33bea2e 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -108,6 +108,11 @@ int PhysicsLoopBack::getUserConstraintInfo(int constraintUniqueId, struct b3User return m_data->m_physicsClient->getUserConstraintInfo( constraintUniqueId, info); } +int PhysicsLoopBack::getUserConstraintId(int serialIndex) const +{ + return m_data->m_physicsClient->getUserConstraintId(serialIndex); +} + ///todo: move this out of the interface void PhysicsLoopBack::setSharedMemoryKey(int key) { diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index 790571170..cd05027f1 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -51,6 +51,8 @@ public: virtual int getNumUserConstraints() const; virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const; + + virtual int getUserConstraintId(int serialIndex) const; ///todo: move this out of the virtual void setSharedMemoryKey(int key); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 36ce76be3..6cd45271d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3818,6 +3818,63 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; }; + case CMD_RESET_DYNAMIC_INFO: + { + BT_PROFILE("CMD_RESET_DYNAMIC_INFO"); + + if (clientCmd.m_updateFlags & RESET_DYNAMIC_INFO_SET_MASS) + { + int bodyUniqueId = clientCmd.m_resetDynamicInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_resetDynamicInfoArgs.m_linkIndex; + double mass = clientCmd.m_resetDynamicInfoArgs.m_mass; + btAssert(bodyUniqueId >= 0); + btAssert(linkIndex >= -1); + + InteralBodyData* body = m_data->getHandle(bodyUniqueId); + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + if (linkIndex == -1) + { + mb->setBaseMass(mass); + } + else + { + mb->getLink(linkIndex).m_mass = mass; + } + } + } + + if (clientCmd.m_updateFlags & RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION) + { + int bodyUniqueId = clientCmd.m_resetDynamicInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_resetDynamicInfoArgs.m_linkIndex; + double lateralFriction = clientCmd.m_resetDynamicInfoArgs.m_lateralFriction; + btAssert(bodyUniqueId >= 0); + btAssert(linkIndex >= -1); + + InteralBodyData* body = m_data->getHandle(bodyUniqueId); + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + if (linkIndex == -1) + { + mb->getBaseCollider()->setFriction(lateralFriction); + } + else + { + mb->getLinkCollider(linkIndex)->setFriction(lateralFriction); + } + } + } + + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + + break; + }; case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS"); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 9d59ba106..d0f6269a5 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1203,39 +1203,45 @@ public: printf("key[%d]=%d state = %d\n",i,m_args[0].m_keyboardEvents[i].m_keyCode,m_args[0].m_keyboardEvents[i].m_keyState); } */ + double shift=0.1; + + CommonWindowInterface* window = m_guiHelper->getAppInterface()->m_window; + if (window->isModifierKeyPressed(B3G_SHIFT)) + shift=0.01; + if (key=='w' && state) { - gVRTeleportPos1[0]+=0.1; + gVRTeleportPos1[0]+=shift; saveCurrentSettingsVR(); } if (key=='s' && state) { - gVRTeleportPos1[0]-=0.1; + gVRTeleportPos1[0]-=shift; saveCurrentSettingsVR(); } if (key=='a' && state) { - gVRTeleportPos1[1]-=0.1; + gVRTeleportPos1[1]-=shift; saveCurrentSettingsVR(); } if (key=='d' && state) { - gVRTeleportPos1[1]+=0.1; + gVRTeleportPos1[1]+=shift; saveCurrentSettingsVR(); } if (key=='q' && state) { - gVRTeleportPos1[2]+=0.1; + gVRTeleportPos1[2]+=shift; saveCurrentSettingsVR(); } if (key=='e' && state) { - gVRTeleportPos1[2]-=0.1; + gVRTeleportPos1[2]-=shift; saveCurrentSettingsVR(); } if (key=='z' && state) { - gVRTeleportRotZ+=0.1; + gVRTeleportRotZ+=shift; gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); saveCurrentSettingsVR(); } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index ecc302ac2..299b1c804 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -106,6 +106,22 @@ struct BulletDataStreamArgs char m_bodyName[MAX_FILENAME_LENGTH]; }; +enum EnumResetDynamicInfoFlags +{ + RESET_DYNAMIC_INFO_SET_MASS=1, + RESET_DYNAMIC_INFO_SET_COM=2, + RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION=4, +}; + +struct ResetDynamicInfoArgs +{ + int m_bodyUniqueId; + int m_linkIndex; + double m_mass; + double m_COM[3]; + double m_lateralFriction; +}; + struct SetJointFeedbackArgs { int m_bodyUniqueId; @@ -714,6 +730,7 @@ struct SharedMemoryCommand struct MjcfArgs m_mjcfArguments; struct FileArgs m_fileArguments; struct SdfRequestInfoArgs m_sdfRequestInfoArgs; + struct ResetDynamicInfoArgs m_resetDynamicInfoArgs; struct InitPoseArgs m_initPoseArgs; struct SendPhysicsSimulationParameters m_physSimParamArgs; struct BulletDataStreamArgs m_dataStreamArguments; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 5cb71729a..57b44fef6 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -56,6 +56,7 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_KEYBOARD_EVENTS_DATA, CMD_REQUEST_OPENGL_VISUALIZER_CAMERA, CMD_REMOVE_BODY, + CMD_RESET_DYNAMIC_INFO, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -218,6 +219,11 @@ struct b3BodyInfo const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf }; +struct b3DynamicInfo +{ + double m_mass; + double m_localInertialPosition[3]; +}; // copied from btMultiBodyLink.h enum SensorType { diff --git a/examples/pybullet/examples/constraint.py b/examples/pybullet/examples/constraint.py index 040708664..51ac0750a 100644 --- a/examples/pybullet/examples/constraint.py +++ b/examples/pybullet/examples/constraint.py @@ -9,6 +9,8 @@ cubeId = p.loadURDF("cube_small.urdf",0,0,1) p.setGravity(0,0,-10) p.setRealTimeSimulation(1) cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1]) +print cid +print p.getConstraintUniqueId(0) prev=[0,0,1] a=-math.pi while 1: @@ -21,4 +23,4 @@ while 1: orn = p.getQuaternionFromEuler([a,0,0]) p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50) -p.removeConstraint(cid) \ No newline at end of file +p.removeConstraint(cid) diff --git a/examples/pybullet/examples/reset_dynamic_info.py b/examples/pybullet/examples/reset_dynamic_info.py new file mode 100644 index 000000000..126312b07 --- /dev/null +++ b/examples/pybullet/examples/reset_dynamic_info.py @@ -0,0 +1,20 @@ +import pybullet as p +import time +import math + +p.connect(p.GUI) + +p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593]) +p.loadURDF(fileName="cube.urdf",baseOrientation=[0.25882,0,0,0.96593],basePosition=[0,0,2]) +p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4]) +p.resetDynamicInfo(bodyUniqueId=2,linkIndex=-1,mass=0.1) +#p.resetDynamicInfo(bodyUniqueId=2,linkIndex=-1,mass=100.0) +p.setGravity(0,0,-10) +p.setRealTimeSimulation(0) +t=0 +while 1: + t=t+1 + if t > 400: + p.resetDynamicInfo(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01) + time.sleep(.01) + p.stepSimulation() diff --git a/examples/pybullet/gym/agents/__init__.py b/examples/pybullet/gym/agents/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/examples/pybullet/gym/agents/actor_net.py b/examples/pybullet/gym/agents/actor_net.py new file mode 100644 index 000000000..ac6aaff8a --- /dev/null +++ b/examples/pybullet/gym/agents/actor_net.py @@ -0,0 +1,21 @@ +"""An actor network.""" +import tensorflow as tf +import sonnet as snt + +class ActorNetwork(snt.AbstractModule): + """An actor network as a sonnet Module.""" + + def __init__(self, layer_sizes, action_size, name='target_actor'): + super(ActorNetwork, self).__init__(name=name) + self._layer_sizes = layer_sizes + self._action_size = action_size + + def _build(self, inputs): + state = inputs + for output_size in self._layer_sizes: + state = snt.Linear(output_size)(state) + state = tf.nn.relu(state) + + action = tf.tanh( + snt.Linear(self._action_size, name='action')(state)) + return action diff --git a/examples/pybullet/gym/agents/simpleAgent.py b/examples/pybullet/gym/agents/simpleAgent.py new file mode 100644 index 000000000..08a4cf1fa --- /dev/null +++ b/examples/pybullet/gym/agents/simpleAgent.py @@ -0,0 +1,46 @@ +"""Loads a DDPG agent without too much external dependencies +""" +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +import os +import collections +import numpy as np +import tensorflow as tf + +import sonnet as snt +from agents import actor_net + +class SimpleAgent(): + def __init__( + self, + session, + ckpt_path, + actor_layer_size, + observation_size=(31,), + action_size=8, + ): + self._ckpt_path = ckpt_path + self._actor_layer_size = actor_layer_size + self._observation_size = observation_size + self._action_size = action_size + self._session = session + self._build() + + def _build(self): + self._agent_net = actor_net.ActorNetwork(self._actor_layer_size, self._action_size) + self._obs = tf.placeholder(tf.float32, (31,)) + with tf.name_scope('Act'): + batch_obs = snt.nest.pack_iterable_as(self._obs, + snt.nest.map(lambda x: tf.expand_dims(x, 0), + snt.nest.flatten_iterable(self._obs))) + self._action = self._agent_net(batch_obs) + saver = tf.train.Saver() + saver.restore( + sess=self._session, + save_path=self._ckpt_path) + + def __call__(self, observation): + out_action = self._session.run(self._action, feed_dict={self._obs: observation}) + return out_action[0] diff --git a/examples/pybullet/gym/data/agent/tf_graph_data/checkpoint b/examples/pybullet/gym/data/agent/tf_graph_data/checkpoint new file mode 100644 index 000000000..2b11c1281 --- /dev/null +++ b/examples/pybullet/gym/data/agent/tf_graph_data/checkpoint @@ -0,0 +1,2 @@ +model_checkpoint_path: "/cns/ij-d/home/jietan/persistent/minitaur/minitaur_vizier_3_153645653/Bullet/MinitaurSimEnv/28158/0003600000/agent/tf_graph_data/tf_graph_data.ckpt" +all_model_checkpoint_paths: "/cns/ij-d/home/jietan/persistent/minitaur/minitaur_vizier_3_153645653/Bullet/MinitaurSimEnv/28158/0003600000/agent/tf_graph_data/tf_graph_data.ckpt" diff --git a/examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data.ckpt.data-00000-of-00001 b/examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data.ckpt.data-00000-of-00001 new file mode 100644 index 000000000..b25aa2872 Binary files /dev/null and b/examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data.ckpt.data-00000-of-00001 differ diff --git a/examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data.ckpt.index b/examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data.ckpt.index new file mode 100644 index 000000000..8abcb6ea5 Binary files /dev/null and b/examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data.ckpt.index differ diff --git a/examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data.ckpt.meta b/examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data.ckpt.meta new file mode 100644 index 000000000..e1369a3d1 Binary files /dev/null and b/examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data.ckpt.meta differ diff --git a/examples/pybullet/gym/envs/bullet/minitaurGymEnv.py b/examples/pybullet/gym/envs/bullet/minitaurGymEnv.py new file mode 100644 index 000000000..c8ca5e536 --- /dev/null +++ b/examples/pybullet/gym/envs/bullet/minitaurGymEnv.py @@ -0,0 +1,109 @@ +import math +import gym +from gym import spaces +from gym.utils import seeding +import numpy as np +import time +import pybullet as p +import minitaur_new + +class MinitaurGymEnv(gym.Env): + metadata = { + 'render.modes': ['human', 'rgb_array'], + 'video.frames_per_second' : 50 + } + + def __init__(self, + urdfRoot="", + actionRepeat=1, + render=False): + self._timeStep = 0.01 + self._urdfRoot = urdfRoot + self._actionRepeat = actionRepeat + self._observation = [] + self._envStepCounter = 0 + self._render = render + self._lastBasePosition = [0, 0, 0] + if self._render: + p.connect(p.GUI) + else: + p.connect(p.DIRECT) + self._seed() + self.reset() + observationDim = self._minitaur.getObservationDimension() + observation_high = np.array([np.finfo(np.float32).max] * observationDim) + actionDim = 8 + action_high = np.array([1] * actionDim) + self.action_space = spaces.Box(-action_high, action_high) + self.observation_space = spaces.Box(-observation_high, observation_high) + self.viewer = None + + def _reset(self): + p.resetSimulation() + p.setPhysicsEngineParameter(numSolverIterations=300) + p.setTimeStep(self._timeStep) + p.loadURDF("%splane.urdf" % self._urdfRoot) + p.setGravity(0,0,-10) + self._minitaur = minitaur_new.Minitaur(self._urdfRoot) + self._envStepCounter = 0 + self._lastBasePosition = [0, 0, 0] + for i in range(100): + p.stepSimulation() + self._observation = self._minitaur.getObservation() + return self._observation + + def __del__(self): + p.disconnect() + + def _seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def _step(self, action): + if (self._render): + basePos = self._minitaur.getBasePosition() + p.resetDebugVisualizerCamera(1, 30, 40, basePos) + + if len(action) != self._minitaur.getActionDimension(): + raise ValueError("We expect {} continuous action not {}.".format(self._minitaur.getActionDimension(), len(action))) + + for i in range(len(action)): + if not -1.01 <= action[i] <= 1.01: + raise ValueError("{}th action should be between -1 and 1 not {}.".format(i, action[i])) + + realAction = self._minitaur.convertFromLegModel(action) + self._minitaur.applyAction(realAction) + for i in range(self._actionRepeat): + p.stepSimulation() + if self._render: + time.sleep(self._timeStep) + self._observation = self._minitaur.getObservation() + if self._termination(): + break + self._envStepCounter += 1 + reward = self._reward() + done = self._termination() + return np.array(self._observation), reward, done, {} + + def _render(self, mode='human', close=False): + return + + def is_fallen(self): + orientation = self._minitaur.getBaseOrientation() + rotMat = p.getMatrixFromQuaternion(orientation) + localUp = rotMat[6:] + return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0 or self._observation[-1] < 0.1 + + def _termination(self): + return self.is_fallen() + + def _reward(self): + currentBasePosition = self._minitaur.getBasePosition() + forward_reward = currentBasePosition[0] - self._lastBasePosition[0] + self._lastBasePosition = currentBasePosition + + energyWeight = 0.001 + energy = np.abs(np.dot(self._minitaur.getMotorTorques(), self._minitaur.getMotorVelocities())) * self._timeStep + energy_reward = energyWeight * energy + reward = forward_reward - energy_reward + return reward diff --git a/examples/pybullet/gym/envs/bullet/minitaur_new.py b/examples/pybullet/gym/envs/bullet/minitaur_new.py new file mode 100644 index 000000000..252c12ed3 --- /dev/null +++ b/examples/pybullet/gym/envs/bullet/minitaur_new.py @@ -0,0 +1,166 @@ +import pybullet as p +import numpy as np +import copy +import math + +class Minitaur: + def __init__(self, urdfRootPath=''): + self.urdfRootPath = urdfRootPath + self.reset() + + def buildJointNameToIdDict(self): + nJoints = p.getNumJoints(self.quadruped) + self.jointNameToId = {} + for i in range(nJoints): + jointInfo = p.getJointInfo(self.quadruped, i) + self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] + self.resetPose() + + def buildMotorIdList(self): + self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint']) + self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint']) + self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint']) + self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint']) + + def reset(self): +# self.quadruped = p.loadURDF("%squadruped/minitaur.urdf" % self.urdfRootPath, [0,0,.2], flags=p.URDF_USE_SELF_COLLISION) + self.quadruped = p.loadURDF("%squadruped/minitaur.urdf" % self.urdfRootPath, [0,0,.2]) + self.kp = 1 + self.kd = 1 + self.maxForce = 3.5 + self.nMotors = 8 + self.motorIdList = [] + self.motorDir = [-1, -1, -1, -1, 1, 1, 1, 1] + self.buildJointNameToIdDict() + self.buildMotorIdList() + + + def setMotorAngleById(self, motorId, desiredAngle): + p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce) + + def setMotorAngleByName(self, motorName, desiredAngle): + self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle) + + def resetPose(self): + kneeFrictionForce = 0 + halfpi = 1.57079632679 + kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length) + + #left front leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle) + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle) + p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) + self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi) + self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + + #left back leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle) + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle) + p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) + self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi) + self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + + #right front leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle) + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle) + p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) + self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi) + self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + + + #right back leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle) + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle) + p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) + self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi) + self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + + + def getBasePosition(self): + position, orientation = p.getBasePositionAndOrientation(self.quadruped) + return position + + def getBaseOrientation(self): + position, orientation = p.getBasePositionAndOrientation(self.quadruped) + return orientation + + def getActionDimension(self): + return self.nMotors + + def getObservationDimension(self): + return len(self.getObservation()) + + def getObservation(self): + observation = [] + observation.extend(self.getMotorAngles().tolist()) + observation.extend(self.getMotorVelocities().tolist()) + observation.extend(self.getMotorTorques().tolist()) + observation.extend(list(self.getBaseOrientation())) + observation.extend(list(self.getBasePosition())) + return observation + + + def applyAction(self, motorCommands): + motorCommandsWithDir = np.multiply(motorCommands, self.motorDir) +# print('action: {}'.format(motorCommands)) +# print('motor: {}'.format(motorCommandsWithDir)) + for i in range(self.nMotors): + self.setMotorAngleById(self.motorIdList[i], motorCommandsWithDir[i]) + + def getMotorAngles(self): + motorAngles = [] + for i in range(self.nMotors): + jointState = p.getJointState(self.quadruped, self.motorIdList[i]) + motorAngles.append(jointState[0]) + motorAngles = np.multiply(motorAngles, self.motorDir) + return motorAngles + + def getMotorVelocities(self): + motorVelocities = [] + for i in range(self.nMotors): + jointState = p.getJointState(self.quadruped, self.motorIdList[i]) + motorVelocities.append(jointState[1]) + motorVelocities = np.multiply(motorVelocities, self.motorDir) + return motorVelocities + + def getMotorTorques(self): + motorTorques = [] + for i in range(self.nMotors): + jointState = p.getJointState(self.quadruped, self.motorIdList[i]) + motorTorques.append(jointState[3]) + motorTorques = np.multiply(motorTorques, self.motorDir) + return motorTorques + + def convertFromLegModel(self, actions): + motorAngle = copy.deepcopy(actions) + scaleForSingularity = 1 + offsetForSingularity = 0.5 + motorAngle[0] = math.pi + math.pi / 4 * actions[0] - scaleForSingularity * math.pi / 4 * (actions[4] + 1 + offsetForSingularity) + motorAngle[1] = math.pi - math.pi / 4 * actions[0] - scaleForSingularity * math.pi / 4 * (actions[4] + 1 + offsetForSingularity) + motorAngle[2] = math.pi + math.pi / 4 * actions[1] - scaleForSingularity * math.pi / 4 * (actions[5] + 1 + offsetForSingularity) + motorAngle[3] = math.pi - math.pi / 4 * actions[1] - scaleForSingularity * math.pi / 4 * (actions[5] + 1 + offsetForSingularity) + motorAngle[4] = math.pi - math.pi / 4 * actions[2] - scaleForSingularity * math.pi / 4 * (actions[6] + 1 + offsetForSingularity) + motorAngle[5] = math.pi + math.pi / 4 * actions[2] - scaleForSingularity * math.pi / 4 * (actions[6] + 1 + offsetForSingularity) + motorAngle[6] = math.pi - math.pi / 4 * actions[3] - scaleForSingularity * math.pi / 4 * (actions[7] + 1 + offsetForSingularity) + motorAngle[7] = math.pi + math.pi / 4 * actions[3] - scaleForSingularity * math.pi / 4 * (actions[7] + 1 + offsetForSingularity) + return motorAngle diff --git a/examples/pybullet/gym/minitaurGymEnvTest.py b/examples/pybullet/gym/minitaurGymEnvTest.py new file mode 100644 index 000000000..ff5db8500 --- /dev/null +++ b/examples/pybullet/gym/minitaurGymEnvTest.py @@ -0,0 +1,85 @@ +''' +A test for minitaurGymEnv +''' + +import gym +import numpy as np +import math + +import numpy as np +import tensorflow as tf + +from envs.bullet.minitaurGymEnv import MinitaurGymEnv +from agents import simpleAgent + +def testSinePolicy(): + """Tests sine policy + """ + np.random.seed(47) + + environment = MinitaurGymEnv(render=True) + sum_reward = 0 + steps = 1000 + amplitude1Bound = 0.5 + amplitude2Bound = 0.15 + speed = 40 + + for stepCounter in range(steps): + t = float(stepCounter) * environment._timeStep + + if (t < 1): + amplitude1 = 0 + amplitude2 = 0 + else: + amplitude1 = amplitude1Bound + amplitude2 = amplitude2Bound + a1 = math.sin(t*speed)*amplitude1 + a2 = math.sin(t*speed+3.14)*amplitude1 + a3 = math.sin(t*speed)*amplitude2 + a4 = math.sin(t*speed+3.14)*amplitude2 + + action = [a1, a2, a2, a1, a3, a4, a4, a3] + + state, reward, done, info = environment.step(action) + sum_reward += reward + if done: + environment.reset() + print("sum reward: ", sum_reward) + + +def testDDPGPolicy(): + """Tests sine policy + """ + environment = MinitaurGymEnv(render=True) + sum_reward = 0 + steps = 1000 + ckpt_path = 'data/agent/tf_graph_data/tf_graph_data.ckpt' + observation_shape = (31,) + action_size = 8 + actor_layer_sizes = (100, 181) + n_steps = 0 + tf.reset_default_graph() + with tf.Session() as session: + agent = simpleAgent.SimpleAgent(session, ckpt_path, + actor_layer_sizes, + observation_size=observation_shape, + action_size=action_size) + state = environment.reset() + action = agent(state) + for _ in range(steps): + n_steps += 1 + state, reward, done, info = environment.step(action) + action = agent(state) + sum_reward += reward + if done: + environment.reset() + n_steps += 1 + print("total reward: ", sum_reward) + print("total steps: ", n_steps) + sum_reward = 0 + n_steps = 0 + return + + +testDDPGPolicy() +#testSinePolicy() diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 6b1c0b884..aaa173924 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -604,6 +604,49 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key return pylist; } +static PyObject* pybullet_resetDynamicInfo(PyObject* self, PyObject* args, PyObject* keywds) +{ + int bodyUniqueId = -1; + int linkIndex = -2; + double mass = -1; + double lateralFriction = -1; + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &physicsClientId)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryCommandHandle command = b3InitResetDynamicInfo(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (mass >= 0) + { + b3ResetDynamicInfoSetMass(command, bodyUniqueId, linkIndex, mass); + } + + if (lateralFriction >= 0) + { + b3ResetDynamicInfoSetLateralFriction(command, bodyUniqueId, linkIndex, lateralFriction); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + } + + Py_INCREF(Py_None); + return Py_None; +} + static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject* keywds) { double fixedTimeStep = -1; @@ -1843,6 +1886,36 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb return NULL; } +static PyObject* pybullet_getConstraintUniqueId(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + int serialIndex = -1; + b3PhysicsClientHandle sm = 0; + + static char* kwlist[] = {"serialIndex", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &serialIndex, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + int userConstraintId = -1; + userConstraintId = b3GetUserConstraintId(sm, serialIndex); + +#if PY_MAJOR_VERSION >= 3 + return PyLong_FromLong(userConstraintId); +#else + return PyInt_FromLong(userConstraintId); +#endif + } +} + static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyObject* keywds) { int numConstraints = 0; @@ -5430,7 +5503,7 @@ static PyMethodDef SpamMethods[] = { {"getConstraintInfo", (PyCFunction)pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS, "Get the user-created constraint info, given a constraint unique id."}, - {"getConstraintUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS | METH_KEYWORDS, + {"getConstraintUniqueId", (PyCFunction)pybullet_getConstraintUniqueId, METH_VARARGS | METH_KEYWORDS, "Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."}, {"getBasePositionAndOrientation", (PyCFunction)pybullet_getBasePositionAndOrientation, @@ -5472,6 +5545,9 @@ static PyMethodDef SpamMethods[] = { {"resetJointState", (PyCFunction)pybullet_resetJointState, METH_VARARGS | METH_KEYWORDS, "Reset the state (position, velocity etc) for a joint on a body " "instantaneously, not through physics simulation."}, + + {"resetDynamicInfo", (PyCFunction)pybullet_resetDynamicInfo, METH_VARARGS | METH_KEYWORDS, + "Reset dynamic information such as mass, lateral friction coefficient."}, {"setJointMotorControl", (PyCFunction)pybullet_setJointMotorControl, METH_VARARGS, "This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead." diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index ea5e90521..8d89cac7d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -140,6 +140,11 @@ public: return m_baseCollider; } + btMultiBodyLinkCollider* getLinkCollider(int index) + { + return m_colliders[index]; + } + // // get parent // input: link num from 0 to num_links-1