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-09-27 15:20:11 -07:00
parent 113e103bc2 8e08a9ff16
commit 85be3b43dd
75 changed files with 6253 additions and 767 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
examples/pybullet/CMakeLists.txt
View File

@@ -43,6 +43,9 @@ SET(pybullet_SRCS
../../examples/SharedMemory/PhysicsDirectC_API.h
../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
../../examples/SharedMemory/PhysicsServerCommandProcessor.h
../../examples/SharedMemory/b3PluginManager.cpp
../../examples/SharedMemory/b3PluginManager.h
../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
../../examples/SharedMemory/PhysicsClientSharedMemory.h
../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp

3
examples/pybullet/examples/mimicJointConstraint.py
View File

@@ -23,6 +23,7 @@ p.changeConstraint(c,gearRatio=-1, maxForce=10000)
p.setRealTimeSimulation(1)
while(1):
p.setGravity(0,0,-10)
time.sleep(0.01)
#p.removeConstraint(c)

1
examples/pybullet/examples/quadruped.py
View File

@@ -187,6 +187,7 @@ t = 0.0
t_end = t + 15
ref_time = time.time()
while (t<t_end):
p.setGravity(0,0,-10)
if (useRealTime):
t = time.time()-ref_time
else:

10
examples/pybullet/examples/vr_kuka_setup.py
View File

@@ -1,6 +1,8 @@
import pybullet as p
import time
#p.connect(p.UDP,"192.168.86.100")
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
@@ -80,6 +82,12 @@ p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setGravity(0.000000,0.000000,0.000000)
p.setGravity(0,0,-10)
p.stepSimulation()
##show this for 10 seconds
#now = time.time()
#while (time.time() < now+10):
# p.stepSimulation()
p.setRealTimeSimulation(1)
while (1):
p.setGravity(0,0,-10)
p.disconnect()

107
examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py Normal file
View File

@@ -0,0 +1,107 @@
import pybullet as p
import time
#p.connect(p.UDP,"192.168.86.100")
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
#objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)]
pr2_gripper = objects[0]
print ("pr2_gripper=")
print (pr2_gripper)
jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
for jointIndex in range (p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(pr2_gripper,jointIndex,p.POSITION_CONTROL,targetPosition=0,force=0)
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
print ("pr2_cid")
print (pr2_cid)
pr2_cid2 = p.createConstraint(pr2_gripper,0,pr2_gripper,2,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(pr2_cid2,gearRatio=1, erp=0.5, relativePositionTarget=0.5, maxForce=3)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
kuka = objects[0]
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
for jointIndex in range (p.getNumJoints(kuka)):
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)]
objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
kuka_gripper = objects[0]
print ("kuka gripper=")
print(kuka_gripper)
p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970])
jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ]
for jointIndex in range (p.getNumJoints(kuka_gripper)):
p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(kuka_gripper,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0])
objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("teddy_vhacd.urdf", 1.050000,-0.500000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("duck_vhacd.urdf", 0.850000,-0.400000,0.900000,0.000000,0.000000,0.707107,0.707107)]
objects = p.loadSDF("kiva_shelf/model.sdf")
ob = objects[0]
p.resetBasePositionAndOrientation(ob,[0.000000,1.000000,1.204500],[0.000000,0.000000,0.000000,1.000000])
objects = [p.loadURDF("teddy_vhacd.urdf", -0.100000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("sphere_small.urdf", -0.100000,0.955006,1.169706,0.633232,-0.000000,-0.000000,0.773962)]
objects = [p.loadURDF("cube_small.urdf", 0.300000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("table_square/table_square.urdf", -1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
ob = objects[0]
jointPositions=[ 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
objects = [p.loadURDF("husky/husky.urdf", 2.000000,-5.000000,1.000000,0.000000,0.000000,0.000000,1.000000)]
ob = objects[0]
jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setGravity(0.000000,0.000000,0.000000)
p.setGravity(0,0,-10)
##show this for 10 seconds
#now = time.time()
#while (time.time() < now+10):
# p.stepSimulation()
p.setRealTimeSimulation(1)
plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll")
controllerId = 3
p.executePluginCommand(plugin ,"bla", [controllerId,pr2_cid, pr2_cid2,pr2_gripper],[50,3])
while (1):
#b = p.getJointState(pr2_gripper,2)[0]
#print("b = " + str(b))
#p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
p.setGravity(0,0,-10)
p.disconnect()

2
examples/pybullet/gym/pybullet_envs/agents/__init__.py Normal file
View File

@@ -0,0 +1,2 @@

110
examples/pybullet/gym/pybullet_envs/agents/config_ppo.py Normal file
View File

@@ -0,0 +1,110 @@
"""The PPO training configuration file for minitaur environments."""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import functools
from agents import ppo
from agents.scripts import networks
from pybullet_envs.bullet import minitaur_gym_env
from pybullet_envs.bullet import minitaur_env_randomizer
import pybullet_envs.bullet.minitaur_gym_env as minitaur_gym_env
import pybullet_envs
# pylint: disable=unused-variable
def default():
"""The default configurations."""
# General
algorithm = ppo.PPOAlgorithm
num_agents = 25
eval_episodes = 25
use_gpu = False
# Network
network = networks.ForwardGaussianPolicy
weight_summaries = dict(
all=r'.*', policy=r'.*/policy/.*', value=r'.*/value/.*')
policy_layers = 200, 100
value_layers = 200, 100
init_mean_factor = 0.2
init_logstd = -1
network_config = dict()
# Optimization
update_every = 25
policy_optimizer = 'AdamOptimizer'
value_optimizer = 'AdamOptimizer'
update_epochs_policy = 25
update_epochs_value = 25
value_lr = 1e-3
policy_lr = 1e-4
# Losses
discount = 0.99
kl_target = 1e-2
kl_cutoff_factor = 2
kl_cutoff_coef = 1000
kl_init_penalty = 1
return locals()
def pybullet_pendulum():
locals().update(default())
env = 'InvertedPendulumBulletEnv-v0'
max_length = 200
steps = 5e7 # 50M
return locals()
def pybullet_doublependulum():
locals().update(default())
env = 'InvertedDoublePendulumBulletEnv-v0'
max_length = 1000
steps = 5e7 # 50M
return locals()
def pybullet_pendulumswingup():
locals().update(default())
env = 'InvertedPendulumSwingupBulletEnv-v0'
max_length = 1000
steps = 5e7 # 50M
return locals()
def pybullet_cheetah():
"""Configuration for MuJoCo's half cheetah task."""
locals().update(default())
# Environment
env = 'HalfCheetahBulletEnv-v0'
max_length = 1000
steps = 1e8 # 100M
return locals()
def pybullet_ant():
locals().update(default())
env = 'AntBulletEnv-v0'
max_length = 1000
steps = 5e7 # 50M
return locals()
def pybullet_racecar():
"""Configuration for Bullet MIT Racecar task."""
locals().update(default())
# Environment
env = 'RacecarBulletEnv-v0' #functools.partial(racecarGymEnv.RacecarGymEnv, isDiscrete=False, renders=True)
max_length = 10
steps = 1e7 # 10M
return locals()
def pybullet_minitaur():
"""Configuration specific to minitaur_gym_env.MinitaurBulletEnv class."""
locals().update(default())
randomizer = (minitaur_env_randomizer.MinitaurEnvRandomizer())
env = functools.partial(
minitaur_gym_env.MinitaurBulletEnv,
accurate_motor_model_enabled=True,
motor_overheat_protection=True,
pd_control_enabled=True,
env_randomizer=randomizer,
render=False)
max_length = 1000
steps = 3e7 # 30M
return locals()

48
examples/pybullet/gym/pybullet_envs/agents/train_ppo.py Normal file
View File

@@ -0,0 +1,48 @@
r"""Script to use Proximal Policy Gradient for the minitaur environments.
Run:
python train_ppo.py --logdif=/tmp/train --config=minitaur_pybullet
"""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import datetime
import os
import tensorflow as tf
from agents import tools
from agents.scripts import train
from agents.scripts import utility
from . import config_ppo
flags = tf.app.flags
FLAGS = tf.app.flags.FLAGS
flags.DEFINE_string(
'logdir', None,
'Base directory to store logs.')
flags.DEFINE_string(
'config', None,
'Configuration to execute.')
flags.DEFINE_string(
'timestamp', datetime.datetime.now().strftime('%Y%m%dT%H%M%S'),
'Sub directory to store logs.')
def main(_):
"""Create or load configuration and launch the trainer."""
config = tools.AttrDict(getattr(config_ppo, FLAGS.config)())
logdir = FLAGS.logdir and os.path.join(
FLAGS.logdir, '{}-{}'.format(FLAGS.timestamp, FLAGS.config))
utility.save_config(config, logdir)
for score in train.train(config, env_processes=True):
tf.logging.info(str(score))
if __name__ == '__main__':
tf.app.run()

42
examples/pybullet/gym/pybullet_envs/agents/visualize_ppo.py Normal file
View File

@@ -0,0 +1,42 @@
r"""Script to visualize the trained PPO agent.
python -m pybullet_envs.agents.visualize \
--logdir=ppo
--outdir=/tmp/video/
"""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import tensorflow as tf
from agents.scripts import visualize
flags = tf.app.flags
FLAGS = tf.app.flags.FLAGS
flags.DEFINE_string("logdir", None,
"Directory to the checkpoint of a training run.")
flags.DEFINE_string("outdir", None,
"Local directory for storing the monitoring outdir.")
flags.DEFINE_string("checkpoint", None,
"Checkpoint name to load; defaults to most recent.")
flags.DEFINE_integer("num_agents", 1,
"How many environments to step in parallel.")
flags.DEFINE_integer("num_episodes", 1, "Minimum number of episodes to render.")
flags.DEFINE_boolean(
"env_processes", False,
"Step environments in separate processes to circumvent the GIL.")
def main(_):
visualize.visualize(FLAGS.logdir, FLAGS.outdir, FLAGS.num_agents,
FLAGS.num_episodes, FLAGS.checkpoint, FLAGS.env_processes)
if __name__ == "__main__":
tf.app.run()

25
examples/pybullet/gym/pybullet_envs/bullet/env_randomizer_base.py Normal file
View File

@@ -0,0 +1,25 @@
"""Abstract base class for environment randomizer."""
import abc
class EnvRandomizerBase(object):
"""Abstract base class for environment randomizer.
An EnvRandomizer is called in environment.reset(). It will
randomize physical parameters of the objects in the simulation.
The physical parameters will be fixed for that episode and be
randomized again in the next environment.reset().
"""
__metaclass__ = abc.ABCMeta
@abc.abstractmethod
def randomize_env(self, env):
"""Randomize the simulated_objects in the environment.
Args:
env: The environment to be randomized.
"""
pass

68
examples/pybullet/gym/pybullet_envs/bullet/minitaur_env_randomizer.py Normal file
View File

@@ -0,0 +1,68 @@
"""Randomize the minitaur_gym_env when reset() is called."""
import random
import numpy as np
from . import env_randomizer_base
# Relative range.
MINITAUR_BASE_MASS_ERROR_RANGE = (-0.2, 0.2) # 0.2 means 20%
MINITAUR_LEG_MASS_ERROR_RANGE = (-0.2, 0.2) # 0.2 means 20%
# Absolute range.
BATTERY_VOLTAGE_RANGE = (14.8, 16.8) # Unit: Volt
MOTOR_VISCOUS_DAMPING_RANGE = (0, 0.01) # Unit: N*m*s/rad (torque/angular vel)
MINITAUR_LEG_FRICTION = (0.8, 1.5) # Unit: dimensionless
class MinitaurEnvRandomizer(env_randomizer_base.EnvRandomizerBase):
"""A randomizer that change the minitaur_gym_env during every reset."""
def __init__(self,
minitaur_base_mass_err_range=MINITAUR_BASE_MASS_ERROR_RANGE,
minitaur_leg_mass_err_range=MINITAUR_LEG_MASS_ERROR_RANGE,
battery_voltage_range=BATTERY_VOLTAGE_RANGE,
motor_viscous_damping_range=MOTOR_VISCOUS_DAMPING_RANGE):
self._minitaur_base_mass_err_range = minitaur_base_mass_err_range
self._minitaur_leg_mass_err_range = minitaur_leg_mass_err_range
self._battery_voltage_range = battery_voltage_range
self._motor_viscous_damping_range = motor_viscous_damping_range
def randomize_env(self, env):
self._randomize_minitaur(env.minitaur)
def _randomize_minitaur(self, minitaur):
"""Randomize various physical properties of minitaur.
It randomizes the mass/inertia of the base, mass/inertia of the legs,
friction coefficient of the feet, the battery voltage and the motor damping
at each reset() of the environment.
Args:
minitaur: the Minitaur instance in minitaur_gym_env environment.
"""
base_mass = minitaur.GetBaseMassFromURDF()
randomized_base_mass = random.uniform(
base_mass * (1.0 + self._minitaur_base_mass_err_range[0]),
base_mass * (1.0 + self._minitaur_base_mass_err_range[1]))
minitaur.SetBaseMass(randomized_base_mass)
leg_masses = minitaur.GetLegMassesFromURDF()
leg_masses_lower_bound = np.array(leg_masses) * (
1.0 + self._minitaur_leg_mass_err_range[0])
leg_masses_upper_bound = np.array(leg_masses) * (
1.0 + self._minitaur_leg_mass_err_range[1])
randomized_leg_masses = [
np.random.uniform(leg_masses_lower_bound[i], leg_masses_upper_bound[i])
for i in range(len(leg_masses))
]
minitaur.SetLegMasses(randomized_leg_masses)
randomized_battery_voltage = random.uniform(BATTERY_VOLTAGE_RANGE[0],
BATTERY_VOLTAGE_RANGE[1])
minitaur.SetBatteryVoltage(randomized_battery_voltage)
randomized_motor_damping = random.uniform(MOTOR_VISCOUS_DAMPING_RANGE[0],
MOTOR_VISCOUS_DAMPING_RANGE[1])
minitaur.SetMotorViscousDamping(randomized_motor_damping)
randomized_foot_friction = random.uniform(MINITAUR_LEG_FRICTION[0],
MINITAUR_LEG_FRICTION[1])
minitaur.SetFootFriction(randomized_foot_friction)

3
examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py
View File

@@ -19,6 +19,7 @@ from . import bullet_client
from . import minitaur
import os
import pybullet_data
from . import minitaur_env_randomizer
NUM_SUBSTEPS = 5
NUM_MOTORS = 8
@@ -68,7 +69,7 @@ class MinitaurBulletEnv(gym.Env):
on_rack=False,
render=False,
kd_for_pd_controllers=0.3,
env_randomizer=None):
env_randomizer=minitaur_env_randomizer.MinitaurEnvRandomizer()):
"""Initialize the minitaur gym environment.
Args:

164
examples/pybullet/gym/pybullet_envs/examples/minitaur_gym_env_example.py Normal file
View File

@@ -0,0 +1,164 @@
r"""An example to run of the minitaur gym environment with sine gaits.
"""
import math
import numpy as np
from pybullet_envs.bullet import minitaur_gym_env
import argparse
from pybullet_envs.bullet import minitaur_env_randomizer
def ResetPoseExample():
"""An example that the minitaur stands still using the reset pose."""
steps = 1000
randomizer = (minitaur_env_randomizer.MinitaurEnvRandomizer())
environment = minitaur_gym_env.MinitaurBulletEnv(
render=True,
leg_model_enabled=False,
motor_velocity_limit=np.inf,
pd_control_enabled=True,
accurate_motor_model_enabled=True,
motor_overheat_protection=True,
env_randomizer = randomizer,
hard_reset=False)
action = [math.pi / 2] * 8
for _ in range(steps):
_, _, done, _ = environment.step(action)
if done:
break
environment.reset()
def MotorOverheatExample():
"""An example of minitaur motor overheat protection is triggered.
The minitaur is leaning forward and the motors are getting obove threshold
torques. The overheat protection will be triggered in ~1 sec.
"""
environment = minitaur_gym_env.MinitaurBulletEnv(
render=True,
leg_model_enabled=False,
motor_velocity_limit=np.inf,
motor_overheat_protection=True,
accurate_motor_model_enabled=True,
motor_kp=1.20,
motor_kd=0.00,
on_rack=False)
action = [2.0] * 8
for i in range(8):
action[i] = 2.0 - 0.5 * (-1 if i % 2 == 0 else 1) * (-1 if i < 4 else 1)
steps = 500
actions_and_observations = []
for step_counter in range(steps):
# Matches the internal timestep.
time_step = 0.01
t = step_counter * time_step
current_row = [t]
current_row.extend(action)
observation, _, _, _ = environment.step(action)
current_row.extend(observation.tolist())
actions_and_observations.append(current_row)
environment.reset()
def SineStandExample():
"""An example of minitaur standing and squatting on the floor.
To validate the accurate motor model we command the robot and sit and stand up
periodically in both simulation and experiment. We compare the measured motor
trajectories, torques and gains.
"""
environment = minitaur_gym_env.MinitaurBulletEnv(
render=True,
leg_model_enabled=False,
motor_velocity_limit=np.inf,
motor_overheat_protection=True,
accurate_motor_model_enabled=True,
motor_kp=1.20,
motor_kd=0.02,
on_rack=False)
steps = 1000
amplitude = 0.5
speed = 3
actions_and_observations = []
for step_counter in range(steps):
# Matches the internal timestep.
time_step = 0.01
t = step_counter * time_step
current_row = [t]
action = [math.sin(speed * t) * amplitude + math.pi / 2] * 8
current_row.extend(action)
observation, _, _, _ = environment.step(action)
current_row.extend(observation.tolist())
actions_and_observations.append(current_row)
environment.reset()
def SinePolicyExample():
"""An example of minitaur walking with a sine gait."""
randomizer = (minitaur_env_randomizer.MinitaurEnvRandomizer())
environment = minitaur_gym_env.MinitaurBulletEnv(
render=True,
motor_velocity_limit=np.inf,
pd_control_enabled=True,
hard_reset=False,
env_randomizer = randomizer,
on_rack=False)
sum_reward = 0
steps = 20000
amplitude_1_bound = 0.5
amplitude_2_bound = 0.5
speed = 40
for step_counter in range(steps):
time_step = 0.01
t = step_counter * time_step
amplitude1 = amplitude_1_bound
amplitude2 = amplitude_2_bound
steering_amplitude = 0
if t < 10:
steering_amplitude = 0.5
elif t < 20:
steering_amplitude = -0.5
else:
steering_amplitude = 0
# Applying asymmetrical sine gaits to different legs can steer the minitaur.
a1 = math.sin(t * speed) * (amplitude1 + steering_amplitude)
a2 = math.sin(t * speed + math.pi) * (amplitude1 - steering_amplitude)
a3 = math.sin(t * speed) * amplitude2
a4 = math.sin(t * speed + math.pi) * amplitude2
action = [a1, a2, a2, a1, a3, a4, a4, a3]
_, reward, done, _ = environment.step(action)
sum_reward += reward
if done:
break
environment.reset()
def main():
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--env', help='environment ID (0==sine, 1==stand, 2=reset, 3=overheat)',type=int, default=0)
args = parser.parse_args()
print("--env=" + str(args.env))
if (args.env == 0):
SinePolicyExample()
if (args.env == 1):
SineStandExample()
if (args.env == 2):
ResetPoseExample()
if (args.env == 3):
MotorOverheatExample()
if __name__ == '__main__':
main()

2
examples/pybullet/premake4.lua
View File

@@ -116,6 +116,8 @@ if not _OPTIONS["no-enet"] then
"../../examples/SharedMemory/PhysicsDirectC_API.h",
"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
"../../examples/SharedMemory/b3PluginManager.cpp",
"../../examples/SharedMemory/b3PluginManager.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp",

457
examples/pybullet/pybullet.c
View File

@@ -445,10 +445,10 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
{
printf("Connection terminated, couldn't get body info\n");
b3DisconnectSharedMemory(sm);
sm = 0;
sm = 0;
sPhysicsClients1[freeIndex] = 0;
sPhysicsClientsGUI[freeIndex] = 0;
sNumPhysicsClients++;
sPhysicsClientsGUI[freeIndex] = 0;
sNumPhysicsClients++;
return PyInt_FromLong(-1);
}
}
@@ -2835,7 +2835,7 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
if (info.m_jointName)
{
PyTuple_SetItem(pyListJointInfo, 1,
PyString_FromString(info.m_jointName));
PyString_FromString(info.m_jointName));
} else
{
PyTuple_SetItem(pyListJointInfo, 1,
@@ -3126,13 +3126,14 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject*
int bodyUniqueId = -1;
int linkIndex = -1;
int computeLinkVelocity = 0;
int computeForwardKinematics = 0;
int i;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "computeLinkVelocity", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &linkIndex,&computeLinkVelocity, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "computeLinkVelocity", "computeForwardKinematics", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|iii", kwlist, &bodyUniqueId, &linkIndex,&computeLinkVelocity,&computeForwardKinematics,&physicsClientId))
{
return NULL;
}
@@ -3168,6 +3169,11 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject*
b3RequestActualStateCommandComputeLinkVelocity(cmd_handle,computeLinkVelocity);
}
if (computeForwardKinematics)
{
b3RequestActualStateCommandComputeForwardKinematics(cmd_handle,computeForwardKinematics);
}
status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
@@ -3367,6 +3373,8 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
double textSize = 1.f;
double lifeTime = 0.f;
int physicsClientId = 0;
int debugItemUniqueId = -1;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"text", "textPosition", "textColorRGB", "textSize", "lifeTime", "textOrientation", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL};
@@ -3419,15 +3427,16 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
{
int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
PyObject* item = PyInt_FromLong(debugItemUniqueId);
return item;
debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
}
PyErr_SetString(SpamError, "Error in addUserDebugText.");
return NULL;
{
PyObject* item = PyInt_FromLong(debugItemUniqueId);
return item;
}
}
static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObject* keywds)
@@ -3449,6 +3458,7 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
double lineWidth = 1.f;
double lifeTime = 0.f;
int physicsClientId = 0;
int debugItemUniqueId = -1;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL};
@@ -3492,13 +3502,12 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
{
int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
PyObject* item = PyInt_FromLong(debugItemUniqueId);
return item;
debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
}
{
PyObject* item = PyInt_FromLong(debugItemUniqueId);
return item;
}
PyErr_SetString(SpamError, "Error in addUserDebugLine.");
return NULL;
}
static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject* keywds)
@@ -4754,21 +4763,21 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject*
static PyObject* MyConvertContactPoint(struct b3ContactInformation* contactPointPtr)
{
/*
0 int m_contactFlags;
1 int m_bodyUniqueIdA;
2 int m_bodyUniqueIdB;
3 int m_linkIndexA;
4 int m_linkIndexB;
5 double m_positionOnAInWS[3];//contact point location on object A,
in world space coordinates
6 double m_positionOnBInWS[3];//contact point location on object
A, in world space coordinates
7 double m_contactNormalOnBInWS[3];//the separating contact
normal, pointing from object B towards object A
8 double m_contactDistance;//negative number is penetration, positive
is distance.
9 double m_normalForce;
*/
0 int m_contactFlags;
1 int m_bodyUniqueIdA;
2 int m_bodyUniqueIdB;
3 int m_linkIndexA;
4 int m_linkIndexB;
5 double m_positionOnAInWS[3];//contact point location on object A,
in world space coordinates
6 double m_positionOnBInWS[3];//contact point location on object
A, in world space coordinates
7 double m_contactNormalOnBInWS[3];//the separating contact
normal, pointing from object B towards object A
8 double m_contactDistance;//negative number is penetration, positive
is distance.
9 double m_normalForce;
*/
int i;
@@ -4965,7 +4974,7 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds)
{
static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "gearAuxLink", "physicsClientId", NULL};
static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "gearAuxLink", "relativePositionTarget", "erp", "physicsClientId", NULL};
int userConstraintUniqueId = -1;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
@@ -4979,7 +4988,9 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P
double jointChildFrameOrn[4];
double maxForce = -1;
double gearRatio = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddii", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &gearAuxLink, &physicsClientId))
double relativePositionTarget=1e32;
double erp=-1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddiddi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &gearAuxLink, &relativePositionTarget, &erp, &physicsClientId))
{
return NULL;
}
@@ -5001,6 +5012,16 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P
{
b3InitChangeUserConstraintSetFrameInB(commandHandle, jointChildFrameOrn);
}
if (relativePositionTarget<1e10)
{
b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relativePositionTarget);
}
if (erp>=0)
{
b3InitChangeUserConstraintSetERP(commandHandle, erp);
}
if (maxForce >= 0)
{
b3InitChangeUserConstraintSetMaxForce(commandHandle, maxForce);
@@ -5559,9 +5580,9 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py
{
int bodyUniqueIdA = -1;
int bodyUniqueIdB = -1;
int linkIndexA = -2;
int linkIndexB = -2;
int linkIndexA = -2;
int linkIndexB = -2;
b3SharedMemoryCommandHandle commandHandle;
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
@@ -5583,24 +5604,24 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py
}
commandHandle = b3InitRequestContactPointInformation(sm);
if (bodyUniqueIdA>=0)
{
b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA);
}
if (bodyUniqueIdB>=0)
{
b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB);
}
if (bodyUniqueIdA>=0)
{
b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA);
}
if (bodyUniqueIdB>=0)
{
b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB);
}
if (linkIndexA>=-1)
{
b3SetContactFilterLinkA( commandHandle, linkIndexA);
}
if (linkIndexB >=-1)
{
b3SetContactFilterLinkB( commandHandle, linkIndexB);
}
if (linkIndexA>=-1)
{
b3SetContactFilterLinkA( commandHandle, linkIndexA);
}
if (linkIndexB >=-1)
{
b3SetContactFilterLinkB( commandHandle, linkIndexB);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
@@ -6574,7 +6595,7 @@ static PyObject* pybullet_invertTransform(PyObject* self,
PyErr_SetString(SpamError, "Invalid input: expected position [x,y,z] and orientation [x,y,z,w].");
return NULL;
}
/// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
/// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
@@ -6653,6 +6674,131 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self,
return Py_None;
}
static PyObject* pybullet_loadPlugin(PyObject* self,
PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
char* pluginPath = 0;
b3SharedMemoryCommandHandle command = 0;
b3SharedMemoryStatusHandle statusHandle = 0;
int statusType = -1;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = { "pluginPath", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &pluginPath, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
command = b3CreateCustomCommand(sm);
b3CustomCommandLoadPlugin(command, pluginPath);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusPluginUniqueId(statusHandle);
return PyInt_FromLong(statusType);
}
static PyObject* pybullet_unloadPlugin(PyObject* self,
PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
int pluginUniqueId = -1;
b3SharedMemoryCommandHandle command = 0;
b3SharedMemoryStatusHandle statusHandle = 0;
int statusType = -1;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = { "pluginUniqueId", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &pluginUniqueId,&physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
command = b3CreateCustomCommand(sm);
b3CustomCommandUnloadPlugin(command, pluginUniqueId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
Py_INCREF(Py_None);
return Py_None;;
}
//createCustomCommand for executing commands implemented in a plugin system
static PyObject* pybullet_executePluginCommand(PyObject* self,
PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
int pluginUniqueId = -1;
char* textArgument = 0;
b3SharedMemoryCommandHandle command=0;
b3SharedMemoryStatusHandle statusHandle=0;
int statusType = -1;
PyObject* intArgs=0;
PyObject* floatArgs=0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = { "pluginUniqueId", "textArgument", "intArgs", "floatArgs", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|sOOi", kwlist, &pluginUniqueId, &textArgument, &intArgs, &floatArgs, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
command = b3CreateCustomCommand(sm);
b3CustomCommandExecutePluginCommand(command, pluginUniqueId, textArgument);
{
PyObject* seqIntArgs = intArgs?PySequence_Fast(intArgs, "expected a sequence"):0;
PyObject* seqFloatArgs = floatArgs?PySequence_Fast(floatArgs, "expected a sequence"):0;
int numIntArgs = seqIntArgs?PySequence_Size(intArgs):0;
int numFloatArgs = seqIntArgs?PySequence_Size(floatArgs):0;
int i;
for (i=0;i<numIntArgs;i++)
{
int val = pybullet_internalGetIntFromSequence(seqIntArgs,i);
b3CustomCommandExecuteAddIntArgument(command, val);
}
for (i=0;i<numFloatArgs;i++)
{
float val = pybullet_internalGetFloatFromSequence(seqFloatArgs,i);
b3CustomCommandExecuteAddFloatArgument(command, val);
}
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusPluginCommandResult(statusHandle);
return PyInt_FromLong(statusType);
}
///Inverse Kinematics binding
static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
PyObject* args, PyObject* keywds)
@@ -6847,8 +6993,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
/// Given an object id, joint positions, joint velocities and joint
/// accelerations,
/// compute the joint forces using Inverse Dynamics
static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
PyObject* args, PyObject* keywds)
static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* args, PyObject* keywds)
{
{
int bodyUniqueId;
@@ -6857,14 +7002,21 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
PyObject* objAccelerations;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"bodyUniqueId", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist, &bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "objPositions",
"objVelocities", "objAccelerations",
"physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist,
&bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations,
&physicsClientId))
{
static char* kwlist2[] = {"bodyIndex", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL};
static char* kwlist2[] = {"bodyIndex", "objPositions",
"objVelocities", "objAccelerations",
"physicsClientId", NULL};
PyErr_Clear();
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist2, &bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist2,
&bodyUniqueId, &objPositionsQ, &objVelocitiesQdot,
&objAccelerations, &physicsClientId))
{
return NULL;
}
@@ -6963,34 +7115,188 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
return Py_None;
}
/// Given an object id, joint positions, joint velocities and joint
/// accelerations, compute the Jacobian
static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyObject* keywds)
{
{
int bodyUniqueId;
int linkIndex;
PyObject* localPosition;
PyObject* objPositions;
PyObject* objVelocities;
PyObject* objAccelerations;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "localPosition",
"objPositions", "objVelocities",
"objAccelerations", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOOOO|i", kwlist,
&bodyUniqueId, &linkIndex, &localPosition, &objPositions,
&objVelocities, &objAccelerations, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int szLoPos = PySequence_Size(localPosition);
int szObPos = PySequence_Size(objPositions);
int szObVel = PySequence_Size(objVelocities);
int szObAcc = PySequence_Size(objAccelerations);
int numJoints = b3GetNumJoints(sm, bodyUniqueId);
if (numJoints && (szLoPos == 3) && (szObPos == numJoints) &&
(szObVel == numJoints) && (szObAcc == numJoints))
{
int byteSizeJoints = sizeof(double) * numJoints;
int byteSizeVec3 = sizeof(double) * 3;
int i;
PyObject* pyResultList = PyTuple_New(2);
double* localPoint = (double*)malloc(byteSizeVec3);
double* jointPositions = (double*)malloc(byteSizeJoints);
double* jointVelocities = (double*)malloc(byteSizeJoints);
double* jointAccelerations = (double*)malloc(byteSizeJoints);
double* linearJacobian = (double*)malloc(3 * byteSizeJoints);
double* angularJacobian = (double*)malloc(3 * byteSizeJoints);
pybullet_internalSetVectord(localPosition, localPoint);
for (i = 0; i < numJoints; i++)
{
jointPositions[i] =
pybullet_internalGetFloatFromSequence(objPositions, i);
jointVelocities[i] =
pybullet_internalGetFloatFromSequence(objVelocities, i);
jointAccelerations[i] =
pybullet_internalGetFloatFromSequence(objAccelerations, i);
}
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle =
b3CalculateJacobianCommandInit(sm, bodyUniqueId,
linkIndex, localPoint, jointPositions,
jointVelocities, jointAccelerations);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CALCULATED_JACOBIAN_COMPLETED)
{
int dofCount;
b3GetStatusJacobian(statusHandle, &dofCount, NULL, NULL);
if (dofCount)
{
int byteSizeDofCount = sizeof(double) * dofCount;
double* linearJacobian = (double*)malloc(3 * byteSizeDofCount);
double* angularJacobian = (double*)malloc(3 * byteSizeDofCount);
b3GetStatusJacobian(statusHandle,
NULL,
linearJacobian,
angularJacobian);
if (linearJacobian)
{
int r;
PyObject* pymat = PyTuple_New(3);
for (r = 0; r < 3; ++r) {
int c;
PyObject* pyrow = PyTuple_New(dofCount);
for (c = 0; c < dofCount; ++c) {
int element = r * dofCount + c;
PyTuple_SetItem(pyrow, c,
PyFloat_FromDouble(linearJacobian[element]));
}
PyTuple_SetItem(pymat, r, pyrow);
}
PyTuple_SetItem(pyResultList, 0, pymat);
}
if (angularJacobian)
{
int r;
PyObject* pymat = PyTuple_New(3);
for (r = 0; r < 3; ++r) {
int c;
PyObject* pyrow = PyTuple_New(dofCount);
for (c = 0; c < dofCount; ++c) {
int element = r * dofCount + c;
PyTuple_SetItem(pyrow, c,
PyFloat_FromDouble(angularJacobian[element]));
}
PyTuple_SetItem(pymat, r, pyrow);
}
PyTuple_SetItem(pyResultList, 1, pymat);
}
}
}
else
{
PyErr_SetString(SpamError,
"Internal error in calculateJacobian");
}
}
free(localPoint);
free(jointPositions);
free(jointVelocities);
free(jointAccelerations);
free(linearJacobian);
free(angularJacobian);
if (pyResultList) return pyResultList;
}
else
{
PyErr_SetString(SpamError,
"calculateJacobian [numJoints] needs to be "
"positive, [local position] needs to be of "
"size 3 and [joint positions], "
"[joint velocities], [joint accelerations] "
"need to match the number of joints.");
return NULL;
}
}
}
Py_INCREF(Py_None);
return Py_None;
}
static PyMethodDef SpamMethods[] = {
{"connect", (PyCFunction)pybullet_connectPhysicsServer, METH_VARARGS | METH_KEYWORDS,
"connect(method, key=SHARED_MEMORY_KEY, options='')\n"
"connect(method, hostname='localhost', port=1234, options='')\n"
"Connect to an existing physics server (using shared memory by default)."},
{"disconnect", (PyCFunction)pybullet_disconnectPhysicsServer, METH_VARARGS | METH_KEYWORDS,
"disconnect(physicsClientId=0)\n"
"Disconnect from the physics server."},
{"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS | METH_KEYWORDS,
"resetSimulation(physicsClientId=0)\n"
"Reset the simulation: remove all objects and start from an empty world."},
{"stepSimulation", (PyCFunction)pybullet_stepSimulation, METH_VARARGS | METH_KEYWORDS,
"stepSimulation(physicsClientId=0)\n"
"Step the simulation using forward dynamics."},
{"setGravity", (PyCFunction)pybullet_setGravity, METH_VARARGS | METH_KEYWORDS,
"setGravity(gravX, gravY, gravZ, physicsClientId=0)\n"
"Set the gravity acceleration (x,y,z)."},
{"setTimeStep", (PyCFunction)pybullet_setTimeStep, METH_VARARGS | METH_KEYWORDS,
"setTimeStep(timestep, physicsClientId=0)\n"
"Set the amount of time to proceed at each call to stepSimulation. (unit "
"is seconds, typically range is 0.01 or 0.001)"},
{"setDefaultContactERP", (PyCFunction)pybullet_setDefaultContactERP, METH_VARARGS | METH_KEYWORDS,
"setDefaultContactERP(defaultContactERP, physicsClientId=0)\n"
"Set the amount of contact penetration Error Recovery Paramater "
"(ERP) in each time step. \
This is an tuning parameter to control resting contact stability. "
"This value depends on the time step."},
{"setRealTimeSimulation", (PyCFunction)pybullet_setRealTimeSimulation, METH_VARARGS | METH_KEYWORDS,
"setRealTimeSimulation(enableRealTimeSimulation, physicsClientId=0)\n"
"Enable or disable real time simulation (using the real time clock,"
" RTC) in the physics server. Expects one integer argument, 0 or 1"},
@@ -7001,6 +7307,8 @@ static PyMethodDef SpamMethods[] = {
"This is for experimental purposes, use at own risk, magic may or not happen"},
{"loadURDF", (PyCFunction)pybullet_loadURDF, METH_VARARGS | METH_KEYWORDS,
"bodyUniqueId = loadURDF(fileName, basePosition=[0.,0.,0.], baseOrientation=[0.,0.,0.,1.], "
"useMaximalCoordinates=0, useFixedBase=0, flags=0, globalScaling=1.0, physicsClientId=0)\n"
"Create a multibody by loading a URDF file."},
{"loadSDF", (PyCFunction)pybullet_loadSDF, METH_VARARGS | METH_KEYWORDS,
@@ -7249,6 +7557,19 @@ static PyMethodDef SpamMethods[] = {
"Given an object id, joint positions, joint velocities and joint "
"accelerations, compute the joint forces using Inverse Dynamics"},
{"calculateJacobian", (PyCFunction)pybullet_calculateJacobian, METH_VARARGS | METH_KEYWORDS,
"Compute the jacobian for a specified local position on a body and its kinematics.\n"
"Args:\n"
" bodyIndex - a scalar defining the unique object id.\n"
" linkIndex - a scalar identifying the link containing the local point.\n"
" localPosition - a list of [x, y, z] of the coordinates of the local point.\n"
" objPositions - a list of the joint positions.\n"
" objVelocities - a list of the joint velocities.\n"
" objAccelerations - a list of the joint accelerations.\n"
"Returns:\n"
" linearJacobian - a list of the partial linear velocities of the jacobian.\n"
" angularJacobian - a list of the partial angular velocities of the jacobian.\n"},
{"calculateInverseKinematics", (PyCFunction)pybullet_calculateInverseKinematics,
METH_VARARGS | METH_KEYWORDS,
"Inverse Kinematics bindings: Given an object id, "
@@ -7281,6 +7602,16 @@ static PyMethodDef SpamMethods[] = {
"Cast a batch of rays and return the result for each of the rays (first object hit, if any. or -1) "
"Takes two arguments (list of from_positions [x,y,z] and a list of to_positions [x,y,z] in Cartesian world coordinates"},
{ "loadPlugin", (PyCFunction)pybullet_loadPlugin, METH_VARARGS | METH_KEYWORDS,
"Load a plugin, could implement custom commands etc." },
{ "unloadPlugin", (PyCFunction)pybullet_unloadPlugin, METH_VARARGS | METH_KEYWORDS,
"Unload a plugin, given the pluginUniqueId." },
{ "executePluginCommand", (PyCFunction)pybullet_executePluginCommand, METH_VARARGS | METH_KEYWORDS,
"Execute a command, implemented in a plugin." },
{"submitProfileTiming", (PyCFunction)pybullet_submitProfileTiming, METH_VARARGS | METH_KEYWORDS,
"Add a custom profile timing that will be visible in performance profile recordings on the physics server."
"On the physics server (in GUI and VR mode) you can press 'p' to start and/or stop profile recordings" },

3094
examples/pybullet/unity3d/autogen/NativeMethods.cs Normal file
View File

File diff suppressed because it is too large Load Diff

86
examples/pybullet/unity3d/examples/NewBehaviourScript.cs Normal file
View File

@@ -0,0 +1,86 @@
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.UI;
using System.Runtime.InteropServices;
using System;
[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)]
public class NewBehaviourScript : MonoBehaviour {
[System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3ConnectSharedMemory")]
public static extern System.IntPtr b3ConnectSharedMemory(int key);
[System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3CreateInProcessPhysicsServerAndConnect")]
public static extern System.IntPtr b3CreateInProcessPhysicsServerAndConnect(int argc, ref System.IntPtr argv);
[System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3InitStepSimulationCommand")]
public static extern System.IntPtr b3InitStepSimulationCommand(IntPtr physClient);
[System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3LoadUrdfCommandInit")]
public static extern System.IntPtr b3LoadUrdfCommandInit(IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string urdfFileName);
[System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3InitResetSimulationCommand")]
public static extern System.IntPtr b3InitResetSimulationCommand(IntPtr physClient);
[System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3SubmitClientCommandAndWaitStatus")]
public static extern System.IntPtr b3SubmitClientCommandAndWaitStatus(IntPtr physClient, IntPtr commandHandle);
[DllImport("TestCppPlug.dll")]
static extern int Add(int a, int b);
[DllImport("TestCppPlug.dll")]
static extern int CallMe(Action<int> action);
[DllImport("TestCppPlug.dll")]
static extern IntPtr CreateSharedAPI(int id);
[DllImport("TestCppPlug.dll")]
static extern int GetMyIdPlusTen(IntPtr api);
[DllImport("TestCppPlug.dll")]
static extern void DeleteSharedAPI(IntPtr api);
private void IWasCalled(int value)
{
text.text = value.ToString();
}
Text text;
IntPtr sharedAPI;
IntPtr pybullet;
// Use this for initialization
void Start () {
text = GetComponent<Text>();
CallMe(IWasCalled);
sharedAPI = CreateSharedAPI(30);
IntPtr pybullet = b3ConnectSharedMemory(12347);
IntPtr cmd = b3InitResetSimulationCommand(pybullet);
IntPtr status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
cmd = b3LoadUrdfCommandInit(pybullet, "plane.urdf");
status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
//IntPtr clientPtr = b3CreateInProcessPhysicsServerAndConnect(0, ref ptr);
}
// Update is called once per frame
void Update () {
IntPtr cmd = b3InitStepSimulationCommand(pybullet);
IntPtr status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
//System.IO.Directory.GetCurrentDirectory().ToString();//
//text.text = Add(4, 5).ToString();
text.text = UnityEngine.Random.Range(0f, 1f).ToString();// GetMyIdPlusTen(sharedAPI).ToString();
}
void OnDestroy()
{
DeleteSharedAPI(sharedAPI);
}
}

12
examples/pybullet/unity3d/examples/NewBehaviourScript.cs.meta Normal file
View File

@@ -0,0 +1,12 @@
fileFormatVersion: 2
guid: 6197b3a0389e92c47b7d8698e5f61f06
timeCreated: 1505961790
licenseType: Free
MonoImporter:
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

33
examples/pybullet/unity3d/readme.txt Normal file
View File

@@ -0,0 +1,33 @@
Quick prototype to connect Unity 3D to pybullet
Generate C# Native Methods using the Microsoft PInvoke Signature Toolkit:
sigimp.exe /lang:cs e:\develop\bullet3\examples\SharedMemory\PhysicsClientC_API.h
Add some #define B3_SHARED_API __declspec(dllexport) to the exported methods,
replace [3], [4], [16] by [] to get sigimp.exe working.
This generates autogen/NativeMethods.cs
Then put pybullet.dll in the right location, so Unity finds it.
NewBehaviourScript.cs is a 1 evening prototype that works within Unity 3D:
Create a connection to pybullet, reset the world, load a urdf at startup.
Step the simulation each Update.
Now the real work can start, converting Unity objects to pybullet,
pybullet robots to Unity, synchronizing the transforms each Update.
void Start () {
IntPtr pybullet = b3ConnectSharedMemory(12347);
IntPtr cmd = b3InitResetSimulationCommand(pybullet);
IntPtr status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
cmd = b3LoadUrdfCommandInit(pybullet, "plane.urdf");
status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
}
void Update ()
{
IntPtr cmd = b3InitStepSimulationCommand(pybullet);
IntPtr status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
}