AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

Merge pull request #1377 from erwincoumans/master

Browse Source 
small fixes
This commit is contained in:
erwincoumans 2017-10-10 11:59:25 -07:00 committed by GitHub
commit 6b10690373
10 changed files with 146 additions and 211 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
data/cartpole.urdf
View File

@ -62,6 +62,12 @@
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<collision>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
</collision>
</link>
<joint name="cart_to_pole" type="continuous">

BIN
data/multibody.bullet
View File

Binary file not shown.

2
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

@ -391,7 +391,7 @@ void btShutDownExampleBrowser(btInProcessExampleBrowserInternalData* data)
} else
{
// printf("polling..");
b3Clock::usleep(1000);
b3Clock::usleep(0);
}
};

63
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -1654,6 +1654,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3Ph
B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3])
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
btAssert(status);
if (status==0)
return 0;
const b3SendCollisionInfoArgs &args = status->m_sendCollisionInfoArgs;
btAssert(status->m_type == CMD_REQUEST_COLLISION_INFO_COMPLETED);
if (status->m_type != CMD_REQUEST_COLLISION_INFO_COMPLETED)
@ -1695,6 +1698,9 @@ B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle
const double* actualStateQdot[],
const double* jointReactionForces[]) {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
btAssert(status);
if (status==0)
return 0;
const SendActualStateArgs &args = status->m_sendActualStateArgs;
btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED);
if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
@ -1910,6 +1916,8 @@ B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle stat
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
b3Assert(status);
if (status==0)
return statusUniqueId;
b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
{
@ -1924,10 +1932,13 @@ B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHan
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
if (status)
{
statusUniqueId = status->m_customCommandResultArgs.m_pluginUniqueId;
b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
{
statusUniqueId = status->m_customCommandResultArgs.m_pluginUniqueId;
}
}
return statusUniqueId;
}
@ -2317,12 +2328,14 @@ B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle s
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED);
if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED)
if (status)
{
return status->m_userConstraintResultArgs.m_userConstraintUniqueId;
b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED);
if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED)
{
return status->m_userConstraintResultArgs.m_userConstraintUniqueId;
}
}
return -1;
}
@ -2635,11 +2648,14 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3Physics
B3_SHARED_API int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED);
if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED))
if (status)
{
*paramValue = status->m_userDebugDrawArgs.m_parameterValue;
return 1;
btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED);
if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED))
{
*paramValue = status->m_userDebugDrawArgs.m_parameterValue;
return 1;
}
}
return 0;
}
@ -3280,10 +3296,13 @@ B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHa
{
int uid = -1;
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED);
if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED)
if (status)
{
uid = status->m_loadTextureResultArguments.m_textureUniqueId;
btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED);
if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED)
{
uid = status->m_loadTextureResultArguments.m_textureUniqueId;
}
}
return uid;
}
@ -3420,6 +3439,9 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand
double* jointForces)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status==0)
return false;
btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED);
if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
return false;
@ -3475,6 +3497,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3Physi
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* linearJacobian, double* angularJacobian)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status==0)
return false;
btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED);
if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED)
return false;
@ -3528,6 +3553,9 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3Shar
b3Assert(cl);
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status==0)
return false;
btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED);
if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED)
return false;
@ -3661,9 +3689,12 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu
double* jointPositions)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status);
if (status==0)
return 0;
btAssert(status->m_type == CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED);
if (status->m_type != CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED)
return false;
return 0;
if (dofCount)
@ -3682,7 +3713,7 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu
}
}
return true;
return 1;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient)

157
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -4499,7 +4499,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//store needed info for tinyrenderer
visualHandle->m_localInertiaFrame = localInertiaFrame;
visualHandle->m_visualShape = visualShape;
visualHandle->m_pathPrefix = pathPrefix ? pathPrefix : "";
visualHandle->m_pathPrefix = pathPrefix[0] ? pathPrefix : "";
serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId;
serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED;
@ -7659,7 +7659,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btAlignedObjectArray<double> q_current;
q_current.resize(numDofs);
if (tree)
if (tree && (numDofs == tree->numDoFs()))
{
jacSize = jacobian_linear.size();
// Set jacobian value
@ -7697,92 +7697,93 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
}
}
btAlignedObjectArray<double> q_new;
q_new.resize(numDofs);
int ikMethod = 0;
if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY))
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
}
else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
}
else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
}
else
{
ikMethod = IK2_VEL_DLS;
}
if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
btAlignedObjectArray<double> lower_limit;
btAlignedObjectArray<double> upper_limit;
btAlignedObjectArray<double> joint_range;
btAlignedObjectArray<double> rest_pose;
lower_limit.resize(numDofs);
upper_limit.resize(numDofs);
joint_range.resize(numDofs);
rest_pose.resize(numDofs);
for (int i = 0; i < numDofs; ++i)
{
lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i];
upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i];
joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i];
rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i];
}
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
}
btAlignedObjectArray<double> q_new;
q_new.resize(numDofs);
int ikMethod = 0;
if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY))
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
}
else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
}
else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
}
else
{
ikMethod = IK2_VEL_DLS;
}
btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
btAlignedObjectArray<double> lower_limit;
btAlignedObjectArray<double> upper_limit;
btAlignedObjectArray<double> joint_range;
btAlignedObjectArray<double> rest_pose;
lower_limit.resize(numDofs);
upper_limit.resize(numDofs);
joint_range.resize(numDofs);
rest_pose.resize(numDofs);
for (int i = 0; i < numDofs; ++i)
{
lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i];
upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i];
joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i];
rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i];
}
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
}
btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation;
btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation;
btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin();
btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin();
btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
// Set joint damping coefficents. A small default
// damping constant is added to prevent singularity
// with pseudo inverse. The user can set joint damping
// coefficients differently for each joint. The larger
// the damping coefficient is, the less we rely on
// this joint to achieve the IK target.
btAlignedObjectArray<double> joint_damping;
joint_damping.resize(numDofs,0.5);
if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING)
{
for (int i = 0; i < numDofs; ++i)
{
joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i];
}
}
ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]);
// Set joint damping coefficents. A small default
// damping constant is added to prevent singularity
// with pseudo inverse. The user can set joint damping
// coefficients differently for each joint. The larger
// the damping coefficient is, the less we rely on
// this joint to achieve the IK target.
btAlignedObjectArray<double> joint_damping;
joint_damping.resize(numDofs,0.5);
if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING)
{
for (int i = 0; i < numDofs; ++i)
{
joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i];
}
}
ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]);
double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff);
double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff);
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numDofs;i++)
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
}
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numDofs;i++)
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
}
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
}
}
}
hasStatus = true;

18
examples/SharedMemory/PhysicsServerExample.cpp
View File

@ -1131,7 +1131,7 @@ public:
btAlignedObjectArray<UserDebugText> m_userDebugText;
UserDebugText m_tmpText;
int m_resultUserDebugTextUid;
virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingVisualShapeIndex, int optionFlags)
{
@ -1165,7 +1165,7 @@ public:
m_cs->setSharedParam(1, eGUIUserDebugAddText);
workerThreadWait();
return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId;
return m_resultUserDebugTextUid;
}
btAlignedObjectArray<UserDebugParameter*> m_userDebugParams;
@ -1183,6 +1183,7 @@ public:
}
return 0;
}
int m_userDebugParamUid;
virtual int addUserDebugParameter(const char* txt, double rangeMin, double rangeMax, double startValue)
{
@ -1196,13 +1197,13 @@ public:
m_cs->setSharedParam(1, eGUIUserDebugAddParameter);
workerThreadWait();
return (*m_userDebugParams[m_userDebugParams.size()-1]).m_itemUniqueId;
return m_userDebugParamUid;
}
btAlignedObjectArray<UserDebugDrawLine> m_userDebugLines;
UserDebugDrawLine m_tmpLine;
int m_resultDebugLineUid;
virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex)
{
m_tmpLine.m_lifeTime = lifeTime;
@ -1223,7 +1224,7 @@ public:
m_cs->lock();
m_cs->setSharedParam(1, eGUIUserDebugAddLine);
workerThreadWait();
return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId;
return m_resultDebugLineUid;
}
int m_removeDebugItemUid;
@ -1788,6 +1789,7 @@ void PhysicsServerExample::exitPhysics()
{
for (int i=0;i<MAX_MOTION_NUM_THREADS;i++)
{
m_args[i].m_cs2->unlock();
m_args[i].m_cs->lock();
m_args[i].m_cs->setSharedParam(0,eRequestTerminateMotion);
m_args[i].m_cs->unlock();
@ -1804,7 +1806,7 @@ void PhysicsServerExample::exitPhysics()
} else
{
b3Clock::usleep(1000);
b3Clock::usleep(0);
}
//we need to call 'stepSimulation' to make sure that
//other threads get out of blocking state (workerThreadWait)
@ -2171,6 +2173,7 @@ void PhysicsServerExample::updateGraphics()
B3_PROFILE("eGUIUserDebugAddText");
m_multiThreadedHelper->m_userDebugText.push_back(m_multiThreadedHelper->m_tmpText);
m_multiThreadedHelper->m_resultUserDebugTextUid = m_multiThreadedHelper->m_userDebugText[m_multiThreadedHelper->m_userDebugText.size()-1].m_itemUniqueId;
m_multiThreadedHelper->mainThreadRelease();
break;
}
@ -2190,6 +2193,8 @@ void PhysicsServerExample::updateGraphics()
m_multiThreadedHelper->m_childGuiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_multiThreadedHelper->m_userDebugParamUid = (*m_multiThreadedHelper->m_userDebugParams[m_multiThreadedHelper->m_userDebugParams.size()-1]).m_itemUniqueId;
//also add actual menu
m_multiThreadedHelper->mainThreadRelease();
break;
@ -2199,6 +2204,7 @@ void PhysicsServerExample::updateGraphics()
B3_PROFILE("eGUIUserDebugAddLine");
m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine);
m_multiThreadedHelper->m_resultDebugLineUid = m_multiThreadedHelper->m_userDebugLines[m_multiThreadedHelper->m_userDebugLines.size()-1].m_itemUniqueId;
m_multiThreadedHelper->mainThreadRelease();
break;
}

7
examples/pybullet/gym/pybullet_envs/__init__.py
View File

@ -2,13 +2,6 @@ from gym.envs.registration import registry, register, make, spec
# ------------bullet-------------
register(
id='CartPoleBulletEnv-v0',
entry_point='pybullet_envs.bullet:CartPoleBulletEnv',
timestep_limit=1000,
reward_threshold=950.0,
)
register(
id='MinitaurBulletEnv-v0',
entry_point='pybullet_envs.bullet:MinitaurBulletEnv',

1
examples/pybullet/gym/pybullet_envs/bullet/__init__.py
View File

@ -1,4 +1,3 @@
from pybullet_envs.bullet.cartpole_bullet import CartPoleBulletEnv
from pybullet_envs.bullet.minitaur_gym_env import MinitaurBulletEnv
from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv
from pybullet_envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv

101
examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py
View File

@ -1,101 +0,0 @@
"""
Classic cart-pole system implemented by Rich Sutton et al.
Copied from https://webdocs.cs.ualberta.ca/~sutton/book/code/pole.c
"""
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
import logging
import math
import gym
from gym import spaces
from gym.utils import seeding
import numpy as np
import time
import subprocess
import pybullet as p
import pybullet_data
logger = logging.getLogger(__name__)
class CartPoleBulletEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second' : 50
}
def __init__(self, renders=True):
# start the bullet physics server
self._renders = renders
if (renders):
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
observation_high = np.array([
np.finfo(np.float32).max,
np.finfo(np.float32).max,
np.finfo(np.float32).max,
np.finfo(np.float32).max])
action_high = np.array([0.1])
self.action_space = spaces.Discrete(9)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.theta_threshold_radians = 1
self.x_threshold = 2.4
self._seed()
# self.reset()
self.viewer = None
self._configure()
def _configure(self, display=None):
self.display = display
def _seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def _step(self, action):
p.stepSimulation()
# time.sleep(self.timeStep)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
theta, theta_dot, x, x_dot = self.state
dv = 0.1
deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3]))
done = x < -self.x_threshold \
or x > self.x_threshold \
or theta < -self.theta_threshold_radians \
or theta > self.theta_threshold_radians
reward = 1.0
return np.array(self.state), reward, done, {}
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
self.timeStep = 0.01
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -10)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
p.resetJointState(self.cartpole, 1, initialAngle)
p.resetJointState(self.cartpole, 0, initialCartPos)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
return np.array(self.state)
def _render(self, mode='human', close=False):
return

2
setup.py
View File

@ -441,7 +441,7 @@ print("-----")
setup(
name = 'pybullet',
version='1.5.1',
version='1.5.7',
description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3',