Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai
2017-05-22 22:23:55 -07:00
parent 3eb974f66f 2f3844e5db
commit d383799d97
57 changed files with 1379 additions and 304 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
examples/pybullet/examples/humanoid_knee_position_control.py Normal file
View File

@@ -0,0 +1,37 @@
import pybullet as p
import time
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
cid = p.connect(p.GUI)
p.resetSimulation()
useRealTime = 0
p.setRealTimeSimulation(useRealTime)
p.setGravity(0,0,0)
p.loadURDF("plane.urdf")
obUids = p.loadMJCF("mjcf/humanoid_fixed.xml")
human = obUids[0]
for i in range (p.getNumJoints(human)):
p.setJointMotorControl2(human,i,p.POSITION_CONTROL,targetPosition=0,force=50)
kneeAngleTargetId = p.addUserDebugParameter("kneeAngle",-4,4,-1)
maxForceId = p.addUserDebugParameter("maxForce",0,100,10)
kneeJointIndex=11
while (1):
time.sleep(0.01)
kneeAngleTarget = p.readUserDebugParameter(kneeAngleTargetId)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(human,kneeJointIndex,p.POSITION_CONTROL,targetPosition=kneeAngleTarget,force=maxForce)
if (useRealTime==0):
p.stepSimulation()

21
examples/pybullet/gym/agents/actor_net.py
View File

@@ -1,21 +0,0 @@
"""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

46
examples/pybullet/gym/agents/simpleAgent.py
View File

@@ -1,46 +0,0 @@
"""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]

36
examples/pybullet/gym/agents/simplerAgent.py Normal file
View File

@@ -0,0 +1,36 @@
"""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 pdb
class SimplerAgent():
def __init__(
self,
session,
ckpt_path,
observation_dim=31
):
self._ckpt_path = ckpt_path
self._session = session
self._observation_dim = observation_dim
self._build()
def _build(self):
saver = tf.train.import_meta_graph(self._ckpt_path + '.meta')
saver.restore(
sess=self._session,
save_path=self._ckpt_path)
self._action = tf.get_collection('action_op')[0]
self._obs = tf.get_collection('observation_placeholder')[0]
def __call__(self, observation):
feed_dict={self._obs: observation}
out_action = self._session.run(self._action, feed_dict=feed_dict)
return out_action[0]

4
examples/pybullet/gym/data/agent/tf_graph_data/checkpoint
View File

@@ -1,2 +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"
model_checkpoint_path: "tf_graph_data_converted.ckpt-0"
all_model_checkpoint_paths: "tf_graph_data_converted.ckpt-0"

BIN
examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data.ckpt.data-00000-of-00001
View File

Binary file not shown.

BIN
examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data.ckpt.index
View File

Binary file not shown.

BIN
examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data.ckpt.meta
View File

Binary file not shown.

BIN
examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data_converted.ckpt-0.data-00000-of-00001 Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data_converted.ckpt-0.index Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/data/agent/tf_graph_data/tf_graph_data_converted.ckpt-0.meta Normal file
View File

Binary file not shown.

9
examples/pybullet/gym/minitaurGymEnvTest.py
View File

@@ -10,7 +10,7 @@ import numpy as np
import tensorflow as tf
from envs.bullet.minitaurGymEnv import MinitaurGymEnv
from agents import simpleAgent
from agents import simplerAgent
def testSinePolicy():
"""Tests sine policy
@@ -53,17 +53,14 @@ def testDDPGPolicy():
environment = MinitaurGymEnv(render=True)
sum_reward = 0
steps = 1000
ckpt_path = 'data/agent/tf_graph_data/tf_graph_data.ckpt'
ckpt_path = 'data/agent/tf_graph_data/tf_graph_data_converted.ckpt-0'
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)
agent = simplerAgent.SimplerAgent(session, ckpt_path)
state = environment.reset()
action = agent(state)
for _ in range(steps):

85
examples/pybullet/pybullet.c
View File

@@ -95,6 +95,9 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16])
int i, len;
PyObject* seq;
if (objMat==NULL)
return 0;
seq = PySequence_Fast(objMat, "expected a sequence");
if (seq)
{
@@ -123,6 +126,7 @@ static int pybullet_internalSetVector(PyObject* objVec, float vector[3])
{
int i, len;
PyObject* seq = 0;
if (objVec == NULL)
return 0;
@@ -720,13 +724,15 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
int collisionFilterMode = -1;
double contactBreakingThreshold = -1;
int maxNumCmdPer1ms = -2;
int enableFileCaching = -1;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "physicsClientId", NULL};
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &physicsClientId))
{
return NULL;
}
@@ -777,6 +783,11 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
b3PhysicsParamSetMaxNumCommandsPer1ms(command, maxNumCmdPer1ms);
}
if (enableFileCaching>=0)
{
b3PhysicsParamSetEnableFileCaching(command, enableFileCaching);
}
//ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
@@ -3699,11 +3710,12 @@ static PyObject* pybullet_setVRCameraState(PyObject* self, PyObject* args, PyObj
PyObject* rootPosObj = 0;
PyObject* rootOrnObj = 0;
int trackObjectUid = -2;
int trackObjectFlag = -1;
double rootPos[3];
double rootOrn[4];
static char* kwlist[] = {"rootPosition", "rootOrientation", "trackObject", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|OOii", kwlist, &rootPosObj, &rootOrnObj, &trackObjectUid, &physicsClientId))
static char* kwlist[] = {"rootPosition", "rootOrientation", "trackObject", "trackObjectFlag","physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|OOiii", kwlist, &rootPosObj, &rootOrnObj, &trackObjectUid,&trackObjectFlag, &physicsClientId))
{
return NULL;
}
@@ -3730,6 +3742,11 @@ static PyObject* pybullet_setVRCameraState(PyObject* self, PyObject* args, PyObj
b3SetVRCameraTrackingObject(commandHandle, trackObjectUid);
}
if (trackObjectFlag>=-1)
{
b3SetVRCameraTrackingObjectFlag(commandHandle, trackObjectFlag);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
@@ -4543,7 +4560,7 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject*
b3PhysicsClientHandle sm = 0;
int numJoints = -1;
static char* kwlist[] = {"bodyUniqueId", "jointIndex", "enableSensor", "physicsClientId"};
static char* kwlist[] = {"bodyUniqueId", "jointIndex", "enableSensor", "physicsClientId",NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &jointIndex, &enableSensor, &physicsClientId))
{
@@ -4755,12 +4772,13 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
float projectionMatrix[16];
float lightDir[3];
float lightColor[3];
float lightDist = 10.0;
int hasShadow = 0;
float lightAmbientCoeff = 0.6;
float lightDiffuseCoeff = 0.35;
float lightSpecularCoeff = 0.05;
int renderer = 0;
float lightDist = -1;
int hasShadow = -1;
float lightAmbientCoeff = -1;
float lightDiffuseCoeff = -1;
float lightSpecularCoeff = -1;
int renderer = -1;
// inialize cmd
b3SharedMemoryCommandHandle command;
int physicsClientId = 0;
@@ -4783,12 +4801,12 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
b3RequestCameraImageSetPixelResolution(command, width, height);
// set camera matrices only if set matrix function succeeds
if (pybullet_internalSetMatrix(objViewMat, viewMatrix) && (pybullet_internalSetMatrix(objProjMat, projectionMatrix)))
if (objViewMat && objProjMat && pybullet_internalSetMatrix(objViewMat, viewMatrix) && (pybullet_internalSetMatrix(objProjMat, projectionMatrix)))
{
b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix);
}
//set light direction only if function succeeds
if (pybullet_internalSetVector(lightDirObj, lightDir))
if (lightDirObj && pybullet_internalSetVector(lightDirObj, lightDir))
{
b3RequestCameraImageSetLightDirection(command, lightDir);
}
@@ -4797,16 +4815,34 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
{
b3RequestCameraImageSetLightColor(command, lightColor);
}
if (lightDist>=0)
{
b3RequestCameraImageSetLightDistance(command, lightDist);
}
b3RequestCameraImageSetLightDistance(command, lightDist);
if (hasShadow>=0)
{
b3RequestCameraImageSetShadow(command, hasShadow);
}
if (lightAmbientCoeff>=0)
{
b3RequestCameraImageSetLightAmbientCoeff(command, lightAmbientCoeff);
}
if (lightDiffuseCoeff>=0)
{
b3RequestCameraImageSetLightDiffuseCoeff(command, lightDiffuseCoeff);
}
b3RequestCameraImageSetShadow(command, hasShadow);
if (lightSpecularCoeff>=0)
{
b3RequestCameraImageSetLightSpecularCoeff(command, lightSpecularCoeff);
}
b3RequestCameraImageSetLightAmbientCoeff(command, lightAmbientCoeff);
b3RequestCameraImageSetLightDiffuseCoeff(command, lightDiffuseCoeff);
b3RequestCameraImageSetLightSpecularCoeff(command, lightSpecularCoeff);
b3RequestCameraImageSelectRenderer(command, renderer);//renderer could be ER_BULLET_HARDWARE_OPENGL
if (renderer>=0)
{
b3RequestCameraImageSelectRenderer(command, renderer);//renderer could be ER_BULLET_HARDWARE_OPENGL
}
//PyErr_Clear();
if (b3CanSubmitCommand(sm))
{
@@ -6332,6 +6368,8 @@ initpybullet(void)
PyModule_AddIntConstant(m, "VR_DEVICE_HMD", VR_DEVICE_HMD);
PyModule_AddIntConstant(m, "VR_DEVICE_GENERIC_TRACKER", VR_DEVICE_GENERIC_TRACKER);
PyModule_AddIntConstant(m, "VR_CAMERA_TRACK_OBJECT_ORIENTATION", VR_CAMERA_TRACK_OBJECT_ORIENTATION);
PyModule_AddIntConstant(m, "KEY_IS_DOWN", eButtonIsDown);
PyModule_AddIntConstant(m, "KEY_WAS_TRIGGERED", eButtonTriggered);
PyModule_AddIntConstant(m, "KEY_WAS_RELEASED", eButtonReleased);
@@ -6346,6 +6384,11 @@ initpybullet(void)
PyModule_AddIntConstant(m, "COV_ENABLE_GUI", COV_ENABLE_GUI);
PyModule_AddIntConstant(m, "COV_ENABLE_SHADOWS", COV_ENABLE_SHADOWS);
PyModule_AddIntConstant(m, "COV_ENABLE_WIREFRAME", COV_ENABLE_WIREFRAME);
PyModule_AddIntConstant(m, "COV_ENABLE_VR_PICKING", COV_ENABLE_VR_PICKING);
PyModule_AddIntConstant(m, "COV_ENABLE_VR_TELEPORTING", COV_ENABLE_VR_TELEPORTING);
PyModule_AddIntConstant(m, "COV_ENABLE_RENDERING", COV_ENABLE_RENDERING);
PyModule_AddIntConstant(m, "COV_ENABLE_VR_RENDER_CONTROLLERS", COV_ENABLE_VR_RENDER_CONTROLLERS);
PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER);
PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL);