Merge remote-tracking branch 'bp/master'
This commit is contained in:
115
data/mjcf/humanoid_fixed.xml
Normal file
115
data/mjcf/humanoid_fixed.xml
Normal file
@@ -0,0 +1,115 @@
|
||||
<mujoco model="humanoid">
|
||||
<compiler angle="degree" inertiafromgeom="true"/>
|
||||
<default>
|
||||
<joint armature="1" damping="1" limited="true"/>
|
||||
<geom conaffinity="1" condim="1" contype="1" margin="0.001" material="geom" rgba="0.8 0.6 .4 1"/>
|
||||
<motor ctrllimited="true" ctrlrange="-.4 .4"/>
|
||||
</default>
|
||||
<option integrator="RK4" iterations="50" solver="PGS" timestep="0.003">
|
||||
<!-- <flags solverstat="enable" energy="enable"/>-->
|
||||
</option>
|
||||
<size nkey="5" nuser_geom="1"/>
|
||||
<visual>
|
||||
<map fogend="5" fogstart="3"/>
|
||||
</visual>
|
||||
<asset>
|
||||
<texture builtin="gradient" height="100" rgb1=".4 .5 .6" rgb2="0 0 0" type="skybox" width="100"/>
|
||||
<!-- <texture builtin="gradient" height="100" rgb1="1 1 1" rgb2="0 0 0" type="skybox" width="100"/>-->
|
||||
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
|
||||
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
|
||||
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
|
||||
<material name="geom" texture="texgeom" texuniform="true"/>
|
||||
</asset>
|
||||
<worldbody>
|
||||
<body name="torso" pos="0 0 1.4">
|
||||
<joint armature="0" damping="0" limited="false" name="root" pos="0 0 0" stiffness="0" type="fixed"/>
|
||||
<geom fromto="0 -.07 0 0 .07 0" name="torso1" size="0.07" type="capsule"/>
|
||||
<geom name="head" pos="0 0 .19" size=".09" type="sphere" user="258"/>
|
||||
<geom fromto="-.01 -.06 -.12 -.01 .06 -.12" name="uwaist" size="0.06" type="capsule"/>
|
||||
<body name="lwaist" pos="-.01 0 -0.260" quat="1.000 0 -0.002 0">
|
||||
<geom fromto="0 -.06 0 0 .06 0" name="lwaist" size="0.06" type="capsule"/>
|
||||
<joint armature="0.02" axis="0 0 1" damping="5" name="abdomen_z" pos="0 0 0.065" range="-45 45" stiffness="20" type="hinge"/>
|
||||
<joint armature="0.02" axis="0 1 0" damping="5" name="abdomen_y" pos="0 0 0.065" range="-75 30" stiffness="10" type="hinge"/>
|
||||
<body name="pelvis" pos="0 0 -0.165" quat="1.000 0 -0.002 0">
|
||||
<joint armature="0.02" axis="1 0 0" damping="5" name="abdomen_x" pos="0 0 0.1" range="-35 35" stiffness="10" type="hinge"/>
|
||||
<geom fromto="-.02 -.07 0 -.02 .07 0" name="butt" size="0.09" type="capsule"/>
|
||||
<body name="right_thigh" pos="0 -0.1 -0.04">
|
||||
<joint armature="0.01" axis="1 0 0" damping="5" name="right_hip_x" pos="0 0 0" range="-25 5" stiffness="10" type="hinge"/>
|
||||
<joint armature="0.01" axis="0 0 1" damping="5" name="right_hip_z" pos="0 0 0" range="-60 35" stiffness="10" type="hinge"/>
|
||||
<joint armature="0.01" axis="0 1 0" damping="5" name="right_hip_y" pos="0 0 0" range="-120 20" stiffness="20" type="hinge"/>
|
||||
<geom fromto="0 0 0 0 0.01 -.34" name="right_thigh1" size="0.06" type="capsule"/>
|
||||
<body name="right_shin" pos="0 0.01 -0.403">
|
||||
<joint armature="0.0060" axis="0 -1 0" name="right_knee" pos="0 0 .02" range="-160 -2" stiffness="1" type="hinge"/>
|
||||
<geom fromto="0 0 0 0 0 -.3" name="right_shin1" size="0.049" type="capsule"/>
|
||||
<body name="right_foot" pos="0 0 -0.45">
|
||||
<geom name="right_foot" pos="0 0 0.1" size="0.075" type="sphere" user="0"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="left_thigh" pos="0 0.1 -0.04">
|
||||
<joint armature="0.01" axis="-1 0 0" damping="5" name="left_hip_x" pos="0 0 0" range="-25 5" stiffness="10" type="hinge"/>
|
||||
<joint armature="0.01" axis="0 0 -1" damping="5" name="left_hip_z" pos="0 0 0" range="-60 35" stiffness="10" type="hinge"/>
|
||||
<joint armature="0.01" axis="0 1 0" damping="5" name="left_hip_y" pos="0 0 0" range="-120 20" stiffness="20" type="hinge"/>
|
||||
<geom fromto="0 0 0 0 -0.01 -.34" name="left_thigh1" size="0.06" type="capsule"/>
|
||||
<body name="left_shin" pos="0 -0.01 -0.403">
|
||||
<joint armature="0.0060" axis="0 -1 0" name="left_knee" pos="0 0 .02" range="-160 -2" stiffness="1" type="hinge"/>
|
||||
<geom fromto="0 0 0 0 0 -.3" name="left_shin1" size="0.049" type="capsule"/>
|
||||
<body name="left_foot" pos="0 0 -0.45">
|
||||
<geom name="left_foot" type="sphere" size="0.075" pos="0 0 0.1" user="0" />
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="right_upper_arm" pos="0 -0.17 0.06">
|
||||
<joint armature="0.0068" axis="2 1 1" name="right_shoulder1" pos="0 0 0" range="-85 60" stiffness="1" type="hinge"/>
|
||||
<joint armature="0.0051" axis="0 -1 1" name="right_shoulder2" pos="0 0 0" range="-85 60" stiffness="1" type="hinge"/>
|
||||
<geom fromto="0 0 0 .16 -.16 -.16" name="right_uarm1" size="0.04 0.16" type="capsule"/>
|
||||
<body name="right_lower_arm" pos=".18 -.18 -.18">
|
||||
<joint armature="0.0028" axis="0 -1 1" name="right_elbow" pos="0 0 0" range="-90 50" stiffness="0" type="hinge"/>
|
||||
<geom fromto="0.01 0.01 0.01 .17 .17 .17" name="right_larm" size="0.031" type="capsule"/>
|
||||
<geom name="right_hand" pos=".18 .18 .18" size="0.04" type="sphere"/>
|
||||
</body>
|
||||
</body>
|
||||
<body name="left_upper_arm" pos="0 0.17 0.06">
|
||||
<joint armature="0.0068" axis="2 -1 1" name="left_shoulder1" pos="0 0 0" range="-60 85" stiffness="1" type="hinge"/>
|
||||
<joint armature="0.0051" axis="0 1 1" name="left_shoulder2" pos="0 0 0" range="-60 85" stiffness="1" type="hinge"/>
|
||||
<geom fromto="0 0 0 .16 .16 -.16" name="left_uarm1" size="0.04 0.16" type="capsule"/>
|
||||
<body name="left_lower_arm" pos=".18 .18 -.18">
|
||||
<joint armature="0.0028" axis="0 -1 -1" name="left_elbow" pos="0 0 0" range="-90 50" stiffness="0" type="hinge"/>
|
||||
<geom fromto="0.01 -0.01 0.01 .17 -.17 .17" name="left_larm" size="0.031" type="capsule"/>
|
||||
<geom name="left_hand" pos=".18 -.18 .18" size="0.04" type="sphere"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
<tendon>
|
||||
<fixed name="left_hipknee">
|
||||
<joint coef="-1" joint="left_hip_y"/>
|
||||
<joint coef="1" joint="left_knee"/>
|
||||
</fixed>
|
||||
<fixed name="right_hipknee">
|
||||
<joint coef="-1" joint="right_hip_y"/>
|
||||
<joint coef="1" joint="right_knee"/>
|
||||
</fixed>
|
||||
</tendon>
|
||||
<actuator><!-- this section is not supported, same constants in code -->
|
||||
<motor gear="100" joint="abdomen_y" name="abdomen_y"/>
|
||||
<motor gear="100" joint="abdomen_z" name="abdomen_z"/>
|
||||
<motor gear="100" joint="abdomen_x" name="abdomen_x"/>
|
||||
<motor gear="100" joint="right_hip_x" name="right_hip_x"/>
|
||||
<motor gear="100" joint="right_hip_z" name="right_hip_z"/>
|
||||
<motor gear="300" joint="right_hip_y" name="right_hip_y"/>
|
||||
<motor gear="200" joint="right_knee" name="right_knee"/>
|
||||
<motor gear="100" joint="left_hip_x" name="left_hip_x"/>
|
||||
<motor gear="100" joint="left_hip_z" name="left_hip_z"/>
|
||||
<motor gear="300" joint="left_hip_y" name="left_hip_y"/>
|
||||
<motor gear="200" joint="left_knee" name="left_knee"/>
|
||||
<motor gear="25" joint="right_shoulder1" name="right_shoulder1"/>
|
||||
<motor gear="25" joint="right_shoulder2" name="right_shoulder2"/>
|
||||
<motor gear="25" joint="right_elbow" name="right_elbow"/>
|
||||
<motor gear="25" joint="left_shoulder1" name="left_shoulder1"/>
|
||||
<motor gear="25" joint="left_shoulder2" name="left_shoulder2"/>
|
||||
<motor gear="25" joint="left_elbow" name="left_elbow"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -277,9 +277,10 @@ static ExampleEntry gDefaultExamples[]=
|
||||
ExampleEntry(1,"Gripper Grasp","Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc,eGRIPPER_GRASP),
|
||||
ExampleEntry(1,"Two Point Grasp","Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),
|
||||
ExampleEntry(1,"One Motor Gripper Grasp","Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP),
|
||||
ExampleEntry(1,"Grasp Soft Body","Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),
|
||||
ExampleEntry(1,"Softbody Multibody Coupling","Two way coupling between soft body and multibody experiment", GripperGraspExampleCreateFunc, eSOFTBODY_MULTIBODY_COUPLING),
|
||||
|
||||
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
ExampleEntry(1,"Grasp Soft Body","Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),
|
||||
ExampleEntry(1,"Softbody Multibody Coupling","Two way coupling between soft body and multibody experiment", GripperGraspExampleCreateFunc, eSOFTBODY_MULTIBODY_COUPLING),
|
||||
#endif //USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
|
||||
#ifdef ENABLE_LUA
|
||||
ExampleEntry(1,"Lua Script", "Create the dynamics world, collision shapes and rigid bodies using Lua scripting",
|
||||
|
||||
@@ -588,6 +588,10 @@ public:
|
||||
) :
|
||||
btDiscreteDynamicsWorldMt( dispatcher, pairCache, constraintSolver, collisionConfiguration )
|
||||
{
|
||||
#if USE_PARALLEL_ISLAND_SOLVER
|
||||
btSimulationIslandManagerMt* islandMgr = static_cast<btSimulationIslandManagerMt*>( m_islandManager );
|
||||
islandMgr->setIslandDispatchFunction( parallelIslandDispatch );
|
||||
#endif //#if USE_PARALLEL_ISLAND_SOLVER
|
||||
}
|
||||
|
||||
};
|
||||
@@ -762,14 +766,7 @@ void CommonRigidBodyMTBase::createEmptyDynamicsWorld()
|
||||
#endif //#if USE_PARALLEL_ISLAND_SOLVER
|
||||
btDiscreteDynamicsWorld* world = new MyDiscreteDynamicsWorld( m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration );
|
||||
m_dynamicsWorld = world;
|
||||
|
||||
#if USE_PARALLEL_ISLAND_SOLVER
|
||||
if ( btSimulationIslandManagerMt* islandMgr = dynamic_cast<btSimulationIslandManagerMt*>( world->getSimulationIslandManager() ) )
|
||||
{
|
||||
islandMgr->setIslandDispatchFunction( parallelIslandDispatch );
|
||||
m_multithreadedWorld = true;
|
||||
}
|
||||
#endif //#if USE_PARALLEL_ISLAND_SOLVER
|
||||
m_multithreadedWorld = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
@@ -290,6 +290,7 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
|
||||
b3Warning("Not connected");
|
||||
return;
|
||||
}
|
||||
b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
@@ -1142,5 +1143,5 @@ void b3RobotSimulatorClientAPI::loadBunny(double scale, double mass, double coll
|
||||
b3LoadBunnySetScale(command, scale);
|
||||
b3LoadBunnySetMass(command, mass);
|
||||
b3LoadBunnySetCollisionMargin(command, collisionMargin);
|
||||
b3SubmitClientCommand(m_data->m_physicsClientHandle, command);
|
||||
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
}
|
||||
@@ -62,7 +62,7 @@ SET(SharedMemory_SRCS
|
||||
../Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
|
||||
../Importers/ImportMJCFDemo/BulletMJCFImporter.h
|
||||
../Utils/b3ResourcePath.cpp
|
||||
../Utils/b3Clock.cpp
|
||||
../Utils/b3Clock.cpp
|
||||
../Utils/RobotLoggingUtil.cpp
|
||||
../Utils/RobotLoggingUtil.h
|
||||
../Utils/ChromeTraceUtil.cpp
|
||||
@@ -75,13 +75,13 @@ SET(SharedMemory_SRCS
|
||||
../Importers/ImportSTLDemo/LoadMeshFromSTL.h
|
||||
../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
|
||||
../Importers/ImportColladaDemo/ColladaGraphicsInstance.h
|
||||
../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
|
||||
../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
|
||||
../ThirdPartyLibs/tinyxml/tinystr.cpp
|
||||
../ThirdPartyLibs/tinyxml/tinyxml.cpp
|
||||
../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
|
||||
../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
|
||||
../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
|
||||
../ThirdPartyLibs/stb_image/stb_image.cpp
|
||||
../ThirdPartyLibs/stb_image/stb_image.cpp
|
||||
../MultiThreading/b3ThreadSupportInterface.cpp
|
||||
../MultiThreading/b3ThreadSupportInterface.h
|
||||
)
|
||||
@@ -95,6 +95,10 @@ LINK_LIBRARIES(
|
||||
Bullet3Common BulletWorldImporter BulletFileLoader BulletInverseDynamicsUtils BulletInverseDynamics BulletDynamics BulletCollision LinearMath BussIK
|
||||
)
|
||||
|
||||
IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
|
||||
LINK_LIBRARIES(BulletSoftBody)
|
||||
ENDIF()
|
||||
|
||||
IF (WIN32)
|
||||
ADD_EXECUTABLE(App_PhysicsServer_SharedMemory
|
||||
${SharedMemory_SRCS}
|
||||
@@ -112,7 +116,7 @@ ELSE(WIN32)
|
||||
../MultiThreading/b3PosixThreadSupport.h
|
||||
main.cpp
|
||||
)
|
||||
|
||||
|
||||
ELSE(APPLE)
|
||||
LINK_LIBRARIES( pthread ${DL} )
|
||||
ADD_EXECUTABLE(App_PhysicsServer_SharedMemory
|
||||
@@ -152,7 +156,7 @@ LINK_LIBRARIES(
|
||||
IF (WIN32)
|
||||
ADD_DEFINITIONS(-DGLEW_STATIC)
|
||||
LINK_LIBRARIES( ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} )
|
||||
|
||||
|
||||
ADD_EXECUTABLE(App_PhysicsServer_SharedMemory_GUI
|
||||
${SharedMemory_SRCS}
|
||||
../StandaloneMain/main_opengl_single_example.cpp
|
||||
@@ -169,23 +173,23 @@ ELSE(WIN32)
|
||||
FIND_LIBRARY(COCOA NAMES Cocoa)
|
||||
MESSAGE(${COCOA})
|
||||
LINK_LIBRARIES(${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY})
|
||||
|
||||
|
||||
ADD_EXECUTABLE(App_PhysicsServer_SharedMemory_GUI
|
||||
${SharedMemory_SRCS}
|
||||
../StandaloneMain/main_opengl_single_example.cpp
|
||||
../ExampleBrowser/OpenGLGuiHelper.cpp
|
||||
../ExampleBrowser/GL_ShapeDrawer.cpp
|
||||
../ExampleBrowser/CollisionShape2TriangleMesh.cpp
|
||||
../ExampleBrowser/CollisionShape2TriangleMesh.cpp
|
||||
../MultiThreading/b3PosixThreadSupport.cpp
|
||||
../MultiThreading/b3PosixThreadSupport.h
|
||||
)
|
||||
|
||||
|
||||
ELSE(APPLE)
|
||||
LINK_LIBRARIES( pthread ${DL} )
|
||||
ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1")
|
||||
ADD_DEFINITIONS("-DGLEW_STATIC")
|
||||
ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
|
||||
|
||||
|
||||
ADD_EXECUTABLE(App_PhysicsServer_SharedMemory_GUI
|
||||
${SharedMemory_SRCS}
|
||||
../StandaloneMain/main_opengl_single_example.cpp
|
||||
@@ -215,7 +219,7 @@ INCLUDE_DIRECTORIES(
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/headers
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/samples/shared
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/samples/shared
|
||||
)
|
||||
|
||||
|
||||
@@ -233,7 +237,7 @@ LINK_LIBRARIES(
|
||||
LINK_DIRECTORIES(${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/lib/win64)
|
||||
ELSE(CMAKE_CL_64)
|
||||
LINK_DIRECTORIES(${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/lib/win32)
|
||||
ENDIF(CMAKE_CL_64)
|
||||
ENDIF(CMAKE_CL_64)
|
||||
ADD_EXECUTABLE(App_PhysicsServer_SharedMemory_VR
|
||||
${SharedMemory_SRCS}
|
||||
../StandaloneMain/hellovr_opengl_main.cpp
|
||||
@@ -250,12 +254,12 @@ LINK_LIBRARIES(
|
||||
../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp
|
||||
../ThirdPartyLibs/openvr/samples/shared/pathtools.h
|
||||
../ThirdPartyLibs/openvr/samples/shared/strtools.cpp
|
||||
../ThirdPartyLibs/openvr/samples/shared/strtools.h
|
||||
../ThirdPartyLibs/openvr/samples/shared/strtools.h
|
||||
../ThirdPartyLibs/openvr/samples/shared/Vectors.h
|
||||
../MultiThreading/b3Win32ThreadSupport.cpp
|
||||
../MultiThreading/b3Win32ThreadSupport.h
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/build3/bullet.rc
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
@@ -272,7 +276,7 @@ IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/bin/win32/openvr_api.dll ${CMAKE_CURRENT_BINARY_DIR}/openvr_api.dll
|
||||
)
|
||||
ENDIF(CMAKE_CL_64)
|
||||
|
||||
|
||||
ADD_CUSTOM_COMMAND(
|
||||
TARGET App_PhysicsServer_SharedMemory_VR
|
||||
POST_BUILD
|
||||
|
||||
@@ -3247,6 +3247,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
}
|
||||
case CMD_LOAD_BUNNY:
|
||||
{
|
||||
serverStatusOut.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
|
||||
hasStatus = true;
|
||||
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
double scale = 0.1;
|
||||
double mass = 0.1;
|
||||
@@ -3287,6 +3289,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
psb->getCollisionShape()->setMargin(collisionMargin);
|
||||
|
||||
m_data->m_dynamicsWorld->addSoftBody(psb);
|
||||
serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
||||
37
examples/pybullet/examples/humanoid_knee_position_control.py
Normal file
37
examples/pybullet/examples/humanoid_knee_position_control.py
Normal 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()
|
||||
@@ -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
|
||||
@@ -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
36
examples/pybullet/gym/agents/simplerAgent.py
Normal 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]
|
||||
@@ -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"
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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):
|
||||
|
||||
2
setup.py
2
setup.py
@@ -417,7 +417,7 @@ else:
|
||||
|
||||
setup(
|
||||
name = 'pybullet',
|
||||
version='1.0.6',
|
||||
version='1.0.7',
|
||||
description='Official Python Interface for the Bullet Physics SDK Robotics Simulator',
|
||||
long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine 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',
|
||||
|
||||
Reference in New Issue
Block a user