def hard_reset(self):
p.resetSimulation()
p.setGravity(0, 0, -10)
p.setTimeStep(1 / self.frequency)
p.setRealTimeSimulation(0)
if not self.heavy:
p.loadURDF(URDF(get_scene("plane")).get_path())
# pass
else:
p.loadURDF(URDF(get_scene("plane-big.urdf.xml")).get_path())
def hard_reset(self):
p.resetSimulation()
p.setGravity(0, 0, -10)
p.setTimeStep(1 / self.frequency)
p.setRealTimeSimulation(0)
p.loadURDF(URDF(get_scene("plane")).get_path())
self.addModel("ergojr-pusher")
def addModel(self, robot_model, pose=None):
if pose is None:
pose = [0, 0, 0, 0, 0, 0]
startPos = pose[:3] # RGB = xyz
startOrientation = p.getQuaternionFromEuler(
pose[3:]) # rotated around which axis? # np.deg2rad(90)
# rotating a standing cylinder around the y axis, puts it flat onto the x axis
with self.output_handler():
xml_path = get_scene(robot_model)
if self.new_backlash is not None:
robot_file = self.update_backlash(xml_path)
else:
robot_file = URDF(xml_path, force_recompile=True).get_path()
robot_id = p.loadURDF(
robot_file, startPos, startOrientation, useFixedBase=1)
self.robots.append(robot_id)
if self.debug:
print(robot_model)
for i in range(p.getNumJoints(robot_id)):
print(p.getJointInfo(robot_id, i))
return robot_id
def __init__(self, robot_id, spawn="linear"):
assert spawn in ["linear", "square"]
self.robot_id = robot_id
self.spawn = spawn
self.cube = None
self.dbo = None
xml_path = get_scene("ergojr-gripper-cube")
self.robot_file = URDF(xml_path, force_recompile=True).get_path()
def hard_reset(self):
p.resetSimulation()
p.setGravity(0, 0, -10)
p.setTimeStep(1 / self.frequency)
p.setRealTimeSimulation(0)
# p.loadURDF(URDF(get_scene("plane")).get_path())
new_plane_path = create_xml(self.rank, "plane")
p.loadURDF(URDF(new_plane_path).get_path())
print(self.rank)
self.addModel("ergojr-pusher")
def __init__(self, rank, friction=.4):
self.friction = friction
self.rank = rank
self.puck = None
self.dbo = None
self.target = None
self.goal = None
self.obj_visual = None
xml_new_path = create_xml(self.rank, "ergojr-pusher-puck")
self.robot_file = URDF(xml_new_path, force_recompile=False).get_path()
def __init__(self, friction=.4):
self.friction = friction
self.puck = None
self.dbo = None
self.target = None
self.goal = None
self.obj_visual = None
xml_path = get_scene("ergojr-pusher-puck")
self.robot_file = URDF(xml_path, force_recompile=True).get_path()
def update_backlash(self, xml_path):
backlashes = self.float2list(self.new_backlash)
tree = et.parse(xml_path + ".xacro.xml")
for i in range(6):
bl_val = tree.find(
".//xacro:property[@name='backlash_val{}']".format(i + 1),
NAMESPACE)
bl_val.set('value', "{}".format(backlashes[i]))
filename = "{}-bl{}".format(xml_path, np.around(self.new_backlash, 5))
tree.write(filename + ".xacro.xml")
robot_file = URDF(xml_path, force_recompile=True).get_path()
return robot_file
def addModel(self, robot_model, pose=None):
if pose is None:
pose = [0, 0, 0, 0, 0, 0]
startPos = pose[:3] # RGB = xyz
startOrientation = p.getQuaternionFromEuler(pose[3:])
# rotated around which axis? # np.deg2rad(90)
# rotating a standing cylinder around the y axis, puts it flat onto the x axis
with self.output_handler():
xml_new_path = create_xml(self.rank, robot_model)
robot_file = URDF(xml_new_path, force_recompile=False).get_path()
robot_id = p.loadURDF(
robot_file, startPos, startOrientation, useFixedBase=1)
self.robot = robot_id
# if self.debug:
# print(robot_model)
# for i in range(p.getNumJoints(robot_id)):
# print(p.getJointInfo(robot_id, i))
return robot_id
from gym_ergojr import get_scene
from gym_ergojr.utils.urdf_helper import URDF
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
p.resetDebugVisualizerCamera(cameraDistance=40,
cameraYaw=135,
cameraPitch=-45,
cameraTargetPosition=[0, 0, 0])
p.setGravity(0, 0, -10)
frequency = 100 # Hz
p.setTimeStep(1 / frequency)
p.setRealTimeSimulation(0)
plane = URDF(get_scene("plane-big.urdf.xml")).get_path()
planeId = p.loadURDF(plane)
startPos = [0, 0, 0] # RGB = xyz
startOrientation = p.getQuaternionFromEuler(
[0, 0, 0]) # rotated around which axis? # np.deg2rad(90)
# rotating a standing cylinder around the y axis, puts it flat onto the x axis
xml_path = get_scene("ergojr-penholder-heavy")
robot_file = URDF(xml_path, force_recompile=True).get_path()
robot = p.loadURDF(robot_file, startPos, startOrientation, useFixedBase=1)
for i in range(p.getNumJoints(robot)):
print(p.getJointInfo(robot, i))
#
cameraPitch=-45,
cameraTargetPosition=[0, 0, 0])
p.setGravity(0, 0, -10)
frequency = 100 # Hz
p.setTimeStep(1 / frequency)
p.setRealTimeSimulation(0)
# planeId = p.loadURDF(URDF(get_scene("plane-big.urdf.xml")).get_path())
startPos = [0, 0, 0] # RGB = xyz
startOrientation = p.getQuaternionFromEuler(
[0, 0, 0]) # rotated around which axis? # np.deg2rad(90)
# rotating a standing cylinder around the y axis, puts it flat onto the x axis
robot_file = URDF("urdfs/backlash-test2c.xacro.xml",
force_recompile=True).get_path()
robot = p.loadURDF(robot_file, startPos, startOrientation, useFixedBase=1)
for i in range(p.getNumJoints(robot)):
print(p.getJointInfo(robot, i))
# motors = [0,5]
# motors = [0,3]
motors = [0, 2]
debugParams = []
#
for i in range(len(motors)):
motor = p.addUserDebugParameter("motor{}".format(i + 1), -1, 1, 0)
debugParams.append(motor)
import pybullet as p
import time
import pybullet_data
import numpy as np
from gym_ergojr.utils.urdf_helper import URDF
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
p.setGravity(0, 0, -10)
planeId = p.loadURDF(URDF(get_scene("plane-big.urdf.xml")).get_path())
cubeStartPos = [0, 0, 0.5] # RGB = xyz
cubeStartOrientation = p.getQuaternionFromEuler(
[0, 0, 0]) # rotated around which axis? # np.deg2rad(90)
# rotating a standing cylinder around the y axis, puts it flat onto the x axis
robot_file = URDF("urdfs/tutorial2.urdf", force_recompile=True).get_path()
robot = p.loadURDF(robot_file, cubeStartPos, cubeStartOrientation)
for i in range(p.getNumJoints(robot)):
print(p.getJointInfo(robot, i))
leftWheels = [6, 7]
rightWheels = [2, 3]
leftSlider = p.addUserDebugParameter("leftVelocity", -10, 10, 0)
rightSlider = p.addUserDebugParameter("rightVelocity", -10, 10, 0)
forceSlider = p.addUserDebugParameter("maxForce", -10, 10, 0)
from gym_ergojr.utils.pybullet import DistanceBetweenObjects
from gym_ergojr.utils.urdf_helper import URDF
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
p.resetDebugVisualizerCamera(cameraDistance=0.4,
cameraYaw=135,
cameraPitch=-35,
cameraTargetPosition=[0, 0.05, 0])
p.setGravity(0, 0, -10) # good enough
frequency = 60 # Hz
p.setTimeStep(1 / frequency)
p.setRealTimeSimulation(0)
robot_file = URDF(get_scene("plane"), force_recompile=True).get_path()
robot = p.loadURDF(robot_file)
startPos = [0, 0, 0] # xyz
startOrientation = p.getQuaternionFromEuler(
[0, 0, 0]) # rotated around which axis? # np.deg2rad(90)
xml_path = get_scene("ergojr-gripper")
robot_file = URDF(xml_path, force_recompile=True).get_path()
# Actually load the URDF file into simulation, make the base of the robot unmoving
robot = p.loadURDF(robot_file, startPos, startOrientation, useFixedBase=1)
xml_path = get_scene("ergojr-gripper-cube")
robot_file = URDF(xml_path, force_recompile=True).get_path()
for i in range(p.getNumJoints(robot)):
from gym_ergojr.utils.urdf_helper import URDF
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
# p.setGravity(0, 0, -10)
# planeId = p.loadURDF(URDF(get_scene("plane-big.urdf.xml")).get_path())
cubeStartPos = [0, 0, 0] # RGB = xyz
cubeStartOrientation = p.getQuaternionFromEuler(
[0, 0, 0]) # rotated around which axis? # np.deg2rad(90)
# rotating a standing cylinder around the y axis, puts it flat onto the x axis
robot_file = URDF("urdfs/tutorial2.urdf.xml").get_path()
print("trying to load file:", robot_file)
robot = p.loadURDF(robot_file, cubeStartPos, cubeStartOrientation)
for i in range(p.getNumJoints(robot)):
print(p.getJointInfo(robot, i))
for i in range(240 * 10):
p.stepSimulation()
time.sleep(1. / 240.)
cubePos, cubeOrn = p.getBasePositionAndOrientation(robot)
print(cubePos, cubeOrn)
p.disconnect()
def __init__(self, scaling=0.02):
self.scaling = scaling
xml_path = get_scene("ball")
self.ball_file = URDF(xml_path, force_recompile=False).get_path()
self.hard_reset()
p0 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p0.setAdditionalSearchPath(pybullet_data.getDataPath())
# simulator 1
p1 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p1.setAdditionalSearchPath(pybullet_data.getDataPath())
frequency = 100 # Hz
for p in [p0, p1]:
p.setGravity(0, 0, -10) # good enough
p.setTimeStep(1 / frequency)
p.setRealTimeSimulation(0)
# sim 0 / "p0" is our real environment
p0.loadURDF(URDF(get_scene("plane-alt")).get_path())
# sim 1 / "p1" is our simulated environment that we are trying to match
p1.loadURDF("plane.urdf")
# right now the robot arm is only loaded in the real env, mostly for scale. in the experiments we need to load it in both environments
xml_path = get_scene("ergojr-gripper")
robot_file = URDF(xml_path, force_recompile=True).get_path()
startOrientation = p0.getQuaternionFromEuler(
[0, 0, 0]) # rotated around which axis? # np.deg2rad(90)
robot = p0.loadURDF(robot_file, [0, 0, 0], startOrientation, useFixedBase=1)
motors = [3, 4, 6, 8, 10, 14]
for m in motors:
p0.setJointMotorControl2(robot, m, p.POSITION_CONTROL, targetPosition=0)
scale = .05
from gym_ergojr import get_scene
from gym_ergojr.utils.urdf_helper import URDF
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
p.resetDebugVisualizerCamera(cameraDistance=0.45,
cameraYaw=135,
cameraPitch=-45,
cameraTargetPosition=[0, 0, 0])
p.setGravity(0, 0, -10)
frequency = 100 # Hz
p.setTimeStep(1 / frequency)
p.setRealTimeSimulation(0)
planeId = p.loadURDF(URDF(get_scene("plane-big.urdf.xml")).get_path())
startPos = [0, 0, 0] # RGB = xyz
startOrientation = p.getQuaternionFromEuler(
[0, 0, 0]) # rotated around which axis? # np.deg2rad(90)
# rotating a standing cylinder around the y axis, puts it flat onto the x axis
robot = p.loadURDF("urdfs/PoppyErgoJr_pen.urdf.xml",
startPos,
startOrientation,
useFixedBase=1)
for i in range(p.getNumJoints(robot)):
print(p.getJointInfo(robot, i))
motors = [0, 1, 2, 3, 4, 5]
