p.connect(p.SHARED_MEMORY)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
pr2_gripper = 2
pr2_cid = 1
CONTROLLER_ID = 0
POSITION = 1
ORIENTATION = 2
ANALOG = 3
BUTTONS = 6
gripper_max_joint = 0.550569
while True:
events = p.getVREvents()
for e in (events):
if e[CONTROLLER_ID] == 3: # To make sure we only get the value for one of the remotes
p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500)
p.setJointMotorControl2(pr2_gripper,
0,
controlMode=p.POSITION_CONTROL,
targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,
force=1.0)
p.setJointMotorControl2(pr2_gripper,
2,
controlMode=p.POSITION_CONTROL,
targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,
force=1.1)
def _step(self,action):
done = False
aspect = 1
camTargetPos = [0,0,0]
yaw = 40
pitch = 10.0
roll=0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 240
nearPlane = 0.0001
farPlane = 0.022
lightDirection = [0,1,0]
lightColor = [1,1,1]#optional
agent_po = pb.getBasePositionAndOrientation(self.agent)
x = agent_po[0][0]
y = agent_po[0][1]
z = agent_po[0][2]
if action == 0: #down
x += self.move
elif action == 1: #up
x -= self.move
elif action == 2: #left
y += self.move
elif action == 3: #right
y -= self.move
elif action == 4: #<
z += self.move
elif action == 5: #>
z -= self.move
elif action == 6: # move finger
joint1Pos = pb.getJointState(self.agent, 0)[0]
newJoint1Pos = joint1Pos - 0.01
pb.setJointMotorControl2(self.agent,0,pb.POSITION_CONTROL,newJoint1Pos,force=1)
elif action == 7: # move finger
joint1Pos = pb.getJointState(self.agent, 0)[0]
newJoint1Pos = joint1Pos + 0.01
pb.setJointMotorControl2(self.agent,0,pb.POSITION_CONTROL,newJoint1Pos,force=1)
elif action == 9:
#print(self.current_observation[:100])
#print(np.amax(self.current_observation))
print(np.unique(self.current_observation))
if self.is_touching():
print('I am touching')
else:
print('I am not touching')
pivot = [x,y,z]
orn = pb.getQuaternionFromEuler([0,0,0])
pb.changeConstraint(self.agent_cid,pivot,
jointChildFrameOrientation=orn,
maxForce=50)
posX_viewMatrix = self.posX_view()
posY_viewMatrix = self.posY_view()
posZ_viewMatrix = self.posZ_view()
negY_viewMatrix = self.negY_view()
negZ_viewMatrix = self.negZ_view()
projectionMatrix = pb.computeProjectionMatrixFOV(self.fov,aspect,
nearPlane,farPlane)
#cameraImageHeight = int((2 * 0.851 * self.wandSide)*11800)
#cameraImageHeight = int((2 * 0.851 * self.wandSide)*23600)
w,h,img_arr,depths_posX,mask = pb.getCameraImage(self.cameraImageWidth,
self.cameraImageHeight,
posX_viewMatrix,
projectionMatrix,
lightDirection,
lightColor,
renderer=pb.ER_TINY_RENDERER)
w,h,img_arr,depths_posY,mask = pb.getCameraImage(self.cameraImageWidth,
self.cameraImageHeight,
posY_viewMatrix,
projectionMatrix,
lightDirection,
lightColor,
renderer=pb.ER_TINY_RENDERER)
w,h,img_arr,depths_posZ,mask = pb.getCameraImage(self.cameraImageWidth,
self.cameraImageHeight,
posZ_viewMatrix,
projectionMatrix,
lightDirection,
lightColor,
renderer=pb.ER_TINY_RENDERER)
w,h,img_arr,depths_negY,mask = pb.getCameraImage(self.cameraImageWidth,
self.cameraImageHeight,
negY_viewMatrix,
projectionMatrix,
lightDirection,
lightColor,
renderer=pb.ER_TINY_RENDERER)
w,h,img_arr,depths_negZ,mask = pb.getCameraImage(self.cameraImageWidth,
self.cameraImageHeight,
negZ_viewMatrix,
projectionMatrix,
lightDirection,
lightColor,
renderer=pb.ER_TINY_RENDERER)
info = [42] #answer to life,TODO use real values
pb.stepSimulation()
self.steps += 1
reward = 0
if self.is_touching():
touch_reward = 100000
new_obs_posX = np.absolute(depths_posX - 1.0, dtype=np.float16)
new_obs_posY = np.absolute(depths_posY - 1.0, dtype=np.float16)
new_obs_posZ = np.absolute(depths_posZ - 1.0, dtype=np.float16)
new_obs_negY = np.absolute(depths_negY - 1.0, dtype=np.float16)
new_obs_negZ = np.absolute(depths_negZ - 1.0, dtype=np.float16)
# if you want binary representation of depth camera, uncomment
# the line below
#new_obs[new_obs > 0] = 1
new_obs = np.concatenate((new_obs_posX, new_obs_posY, new_obs_posZ, new_obs_negY, new_obs_negZ))
self.current_observation = new_obs.flatten()
else:
touch_reward = 0
self.current_observation = np.zeros((self.observation_space()))
agent_po = pb.getBasePositionAndOrientation(self.agent)
obj_po = pb.getBasePositionAndOrientation(self.obj_to_classify)
distance = math.sqrt(sum([(xi-yi)**2 for xi,yi in zip(obj_po[0],agent_po[0])])) #TODO faster euclidean
if distance < self.prev_distance:
reward += 1 * (self.max_steps - self.steps)
elif distance > self.prev_distance:
reward -= 10
reward -= distance
reward += touch_reward
self.prev_distance = distance
if self.steps >= self.max_steps or self.is_touching():
done = True
return self.current_observation,reward,done,info
def __init__(self, robot_id, tool_link):
"""Creates suction and 'attaches' it to the robot.
Has special cases when dealing with rigid vs deformables. For rigid,
only need to check contact_constraint for any constraint. For soft
bodies (i.e., cloth or bags), use cloth_threshold to check distances
from gripper body (self.body) to any vertex in the cloth mesh. We
need correct code logic to handle gripping potentially a rigid or a
deformable (and similarly for releasing).
To be clear on terminology: 'deformable' here should be interpreted
as a PyBullet 'softBody', which includes cloths and bags. There's
also cables, but those are formed by connecting rigid body beads, so
they can use standard 'rigid body' grasping code.
To get the suction gripper pose, use p.getLinkState(self.body, 0),
and not p.getBasePositionAndOrientation(self.body) as the latter is
about z=0.03m higher and empirically seems worse.
Args:
robot_id: int representing PyBullet ID of robot.
tool_link: int representing PyBullet ID of tool link.
"""
super().__init__()
position = (0.487, 0.109, 0.351)
rotation = p.getQuaternionFromEuler((np.pi, 0, 0))
urdf = 'assets/ur5/suction/suction-head.urdf'
self.body = p.loadURDF(urdf, position, rotation)
constraint_id = p.createConstraint(parentBodyUniqueId=robot_id,
parentLinkIndex=tool_link,
childBodyUniqueId=self.body,
childLinkIndex=-1,
jointType=p.JOINT_FIXED,
jointAxis=(0, 0, 0),
parentFramePosition=(0, 0, 0),
childFramePosition=(0, 0, -0.07))
p.changeConstraint(constraint_id, maxForce=50)
# Indicates whether gripper is gripping anything (rigid or def).
self.activated = False
# For gripping and releasing rigid objects.
self.contact_constraint = None
# Defaults for deformable parameters, and can override in tasks.
self.def_ignore = 0.035 # TODO(daniel) check if this is needed
self.def_threshold = 0.030
self.def_nb_anchors = 1
# Track which deformable is being gripped (if any), and anchors.
self.def_grip_item = None
self.def_grip_anchors = []
# Determines release when gripped deformable touches a rigid/def.
# TODO(daniel) should check if the code uses this -- not sure?
self.def_min_vetex = None
self.def_min_distance = None
# Determines release when a gripped rigid touches defs (e.g. cloth-cover).
self.init_grip_distance = None
self.init_grip_item = None
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)
def load_agent(self):
dir_path = os.path.dirname(os.path.realpath(__file__))
agent_path = dir_path + "/data/MPL/MPL.xml"
objects = pb.loadMJCF(agent_path, flags=0)
self.agent = objects[0] #1 total
#if self.obj_to_classify:
obj_po = pb.getBasePositionAndOrientation(self.obj_to_classify)
self.hand_cid = pb.createConstraint(self.agent, -1, -1, -1,
pb.JOINT_FIXED, [0, 0, 0],
[0, 0, 0], [0, 0, 0])
#hand_po = pb.getBasePositionAndOrientation(self.agent)
#distance = math.sqrt(sum([(xi-yi)**2 for xi,yi in zip(obj_po[0],hand_po[0])])) #TODO faster euclidean
pb.resetBasePositionAndOrientation(
self.agent, (obj_po[0][0], obj_po[0][1] + 0.5, obj_po[0][2]),
obj_po[1])
hand_po = pb.getBasePositionAndOrientation(self.agent)
ho = pb.getQuaternionFromEuler([0.0, 0.0, 0.0])
pb.changeConstraint(self.hand_cid,
(hand_po[0][0], hand_po[0][1], hand_po[0][2]),
ho,
maxForce=200)
joint7Pos = pb.getJointState(self.agent, 7)[0]
joint24Pos = pb.getJointState(self.agent, 24)[0]
joint32Pos = pb.getJointState(self.agent, 32)[0]
joint40Pos = pb.getJointState(self.agent, 40)[0]
# these are pretty close
joint7tgt = self.pi / 4 # thumb
joint9tgt = self.pi / 2
joint11tgt = self.pi / 2
joint13tgt = self.pi / 2
# pretty close
joint24tgt = self.pi / 2.25 # middle finger
joint26tgt = self.pi / 2
joint28tgt = self.pi / 2
# working
joint32tgt = self.pi / 2.5 # ring finger
joint34tgt = self.pi / 2
joint36tgt = self.pi / 2
# pretty close
joint40tgt = self.pi / 3 # pinky
joint42tgt = self.pi / 2
joint44tgt = self.pi / 2
while (joint7Pos < joint7tgt) and (joint24Pos < joint24tgt) and (
joint32Pos < joint32tgt) and (joint40Pos < joint40tgt):
pb.setJointMotorControl2(self.agent, 7, pb.POSITION_CONTROL,
joint7tgt)
pb.setJointMotorControl2(self.agent, 9, pb.POSITION_CONTROL,
joint9tgt)
pb.setJointMotorControl2(self.agent, 11, pb.POSITION_CONTROL,
joint11tgt)
pb.setJointMotorControl2(self.agent, 13, pb.POSITION_CONTROL,
joint13tgt)
pb.setJointMotorControl2(self.agent, 24, pb.POSITION_CONTROL,
joint24tgt)
pb.setJointMotorControl2(self.agent, 26, pb.POSITION_CONTROL,
joint26tgt)
pb.setJointMotorControl2(self.agent, 28, pb.POSITION_CONTROL,
joint28tgt)
pb.setJointMotorControl2(self.agent, 32, pb.POSITION_CONTROL,
joint32tgt)
pb.setJointMotorControl2(self.agent, 34, pb.POSITION_CONTROL,
joint34tgt)
pb.setJointMotorControl2(self.agent, 36, pb.POSITION_CONTROL,
joint36tgt)
pb.setJointMotorControl2(self.agent, 40, pb.POSITION_CONTROL,
joint40tgt)
pb.setJointMotorControl2(self.agent, 42, pb.POSITION_CONTROL,
joint42tgt)
pb.setJointMotorControl2(self.agent, 44, pb.POSITION_CONTROL,
joint44tgt)
pb.stepSimulation()
joint7Pos = pb.getJointState(self.agent, 7)[0]
joint24Pos = pb.getJointState(self.agent, 24)[0]
joint32Pos = pb.getJointState(self.agent, 32)[0]
joint40Pos = pb.getJointState(self.agent, 40)[0]
erp_lb_Id= p.addUserDebugParameter("erp_lb",0,1,defaultERP)
relPosTarget_lb_Id= p.addUserDebugParameter("relPosTarget_lb",-limitVal,limitVal,0)
camTargetPos=[0.25,0.62,-0.15]
camDist = 2
camYaw = -2
camPitch=-16
p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos)
c_rf = p.createConstraint(vision,knee_front_rightR_joint,vision,motor_front_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,maxForce=maxGearForce)
c_lf = p.createConstraint(vision,knee_front_leftL_joint,vision,motor_front_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,maxForce=maxGearForce)
c_rb = p.createConstraint(vision,knee_back_rightR_joint,vision,motor_back_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,maxForce=maxGearForce)
c_lb = p.createConstraint(vision,knee_back_leftL_joint,vision,motor_back_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_lb,gearRatio=-1, gearAuxLink = motor_back_leftL_joint,maxForce=maxGearForce)
p.setRealTimeSimulation(1)
for i in range (1):
def createWorld(self, model_name, track_name):
self.resetCounter = 0
if model_name is None:
model_name = 'racecar_differential.urdf'
if track_name is None:
track_name = 'barca_track.sdf'
model_path = os.path.join(os.path.dirname(__file__), 'f10_racecar', model_name)
if not os.path.exists(model_path):
raise IOError('Model file {} does not exist'.format(model_path))
track_path = os.path.join(os.path.dirname(__file__), 'f10_racecar/meshes', track_name)
if not os.path.exists(track_path):
raise IOError('Track file {} does not exist'.format(track_path))
self.track = p.loadSDF(track_path, globalScaling=1)
carPosition = [0, 0, 0.15]
carOrientation = p.getQuaternionFromEuler([0, 0, np.pi/3])
# path = os.path.abspath(os.path.dirname(__file__))
self.car = p.loadURDF(model_path, carPosition, carOrientation)
for wheel in range(p.getNumJoints(self.car)):
p.setJointMotorControl2(self.car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
p.getJointInfo(self.car, wheel)
c = p.createConstraint(self.car, 9, self.car, 11, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(self.car, 10, self.car, 13, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(self.car, 9, self.car, 13, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(self.car, 16, self.car, 18, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(self.car, 16, self.car, 19, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(self.car, 17, self.car, 19, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(self.car, 1, self.car, 18, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
c = p.createConstraint(self.car, 3, self.car, 19, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
import pybullet as p
cin = p.connect(p.SHARED_MEMORY)
if (cin < 0):
cin = p.connect(p.GUI)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("quadruped/minitaur_v1.urdf", 1.000397,-0.990977,0.166281,-0.001128,0.001265,0.195322,0.980738)]
ob = objects[0]
jointPositions=[ 0.000000, 1.569646, 0.000000, -2.182872, 1.570201, 0.000000, -2.191865, 1.569370, 0.000000, -2.182832, 1.570121, 0.000000, -2.187810, 0.000000, -1.570037, 0.000000, 2.182983, -1.569349, 0.000000, 2.201204, -1.568805, 0.000000, 2.182471, -1.568983, 0.000000, 2.200243 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
cid0 = p.createConstraint(1,19,1,16,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
p.changeConstraint(cid0,maxForce=1000.000000)
cid1 = p.createConstraint(1,3,1,6,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
p.changeConstraint(cid1,maxForce=1000.000000)
cid2 = p.createConstraint(1,25,1,22,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
p.changeConstraint(cid2,maxForce=1000.000000)
cid3 = p.createConstraint(1,9,1,12,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
p.changeConstraint(cid3,maxForce=1000.000000)
p.setGravity(0.000000,0.000000,-10.000000)
p.stepSimulation()
p.disconnect()
def add_cable_ring(self, env, info={}, cable_idx=0):
"""Add a cable, but make it connected at both ends to form a ring.
For consistency, add each `part_id` to various information tracking
lists and dictionaries (see `add_cable` documentation).
:cable_idx: Used for environments with more than one cable.
:info: Stores relevant stuff, such as for ground-truth targets.
"""
def rad_to_deg(rad):
return (rad * 180.0) / np.pi
def get_discretized_rotations(num_rotations):
# counter-clockwise
theta = i * (2 * np.pi) / num_rotations
return (theta, rad_to_deg(theta))
# Bead properties.
num_parts = self.num_parts
radius = self.radius
color = self.colors[cable_idx] + [1]
# The `ring_radius` (not the bead radius!) has to be tuned somewhat.
# Try to make sure the beads don't have notable gaps between them.
ring_radius = info['ring_radius']
beads = []
bead_positions_l = []
# Add beaded cable. Here, `position` is the circle center.
position = np.float32(info['center_position'])
part_shape = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[radius] * 3)
part_visual = p.createVisualShape(p.GEOM_SPHERE, radius=radius * 1.5)
# Iterate through parts and create constraints as needed.
for i in range(num_parts):
angle_rad, _ = get_discretized_rotations(num_parts)
px = ring_radius * np.cos(angle_rad)
py = ring_radius * np.sin(angle_rad)
#print(f'pos: {px:0.2f}, {py:0.2f}, angle: {angle_rad:0.2f}, {angle_deg:0.1f}')
bead_position = np.float32(
[position[0] + px, position[1] + py, 0.01])
part_id = p.createMultiBody(0.1,
part_shape,
part_visual,
basePosition=bead_position)
p.changeVisualShape(part_id, -1, rgbaColor=color)
if i > 0:
parent_frame = bead_position - bead_positions_l[-1]
constraint_id = p.createConstraint(
parentBodyUniqueId=beads[-1],
parentLinkIndex=-1,
childBodyUniqueId=part_id,
childLinkIndex=-1,
jointType=p.JOINT_POINT2POINT,
jointAxis=(0, 0, 0),
parentFramePosition=parent_frame,
childFramePosition=(0, 0, 0))
p.changeConstraint(constraint_id, maxForce=100)
# Make a constraint with i=0. Careful with `parent_frame`!
if i == num_parts - 1:
parent_frame = bead_positions_l[0] - bead_position
constraint_id = p.createConstraint(
parentBodyUniqueId=part_id,
parentLinkIndex=-1,
childBodyUniqueId=beads[0],
childLinkIndex=-1,
jointType=p.JOINT_POINT2POINT,
jointAxis=(0, 0, 0),
parentFramePosition=parent_frame,
childFramePosition=(0, 0, 0))
p.changeConstraint(constraint_id, maxForce=100)
# Track beads.
beads.append(part_id)
bead_positions_l.append(bead_position)
# The usual for tracking IDs. Four things to add.
self.cable_bead_IDs.append(part_id)
self._IDs[part_id] = f'cable_part_{str(part_id).zfill(2)}'
env.objects.append(part_id)
self.object_points[part_id] = np.float32((0, 0, 0)).reshape(3, 1)
if 'cable-ring' in self._name:
# For targets, we can assume the starting position gives us
# that. Later, apply a random force on the ring to perturb it.
true_position = (bead_position[0], bead_position[1], 0)
self.goal['places'][part_id] = (true_position, (0, 0, 0, 1.))
symmetry = 0
self.goal['steps'][0][part_id] = (symmetry, [part_id])
# Make the true positions visible if desired.
if info['targets_visible']:
sq_pose = ((true_position[0], true_position[1], 0.002),
(0, 0, 0, 1))
sq_template = 'assets/square/square-template-allsides-green.urdf'
replace = {'DIM': (0.003, ), 'HALF': (0.003 / 2, )}
urdf = self.fill_template(sq_template, replace)
env.add_object(urdf, sq_pose, fixed=True)
os.remove(urdf)
else:
print(f'Warning, env {self._name} will not have goals.')
def __init__(self,initial_position=[0,0,0.3]):
self.car=p.loadURDF("f10_racecar/racecar_differential.urdf",
initial_position)
self.action_taken=True
self.wheel_velocity_value=0.0
self.max_force_value=50.0
self.steering_value=0.0
car=self.car
for wheel in range(p.getNumJoints(car)):
print("joint[",wheel,"]=", p.getJointInfo(car,wheel))
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.getJointInfo(self.car,wheel)
c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,1,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000)
c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000)
self.update()
t = 0.0
freq_sim = 240
while 1:
time.sleep(1. / freq_sim)
t += 1. / freq_sim
# some trajectory
ux = amplitude_x * math.sin(2 * math.pi * freq * t)
uy = amplitude_y * math.cos(2 * math.pi * freq * t)
# move base arround
pivot = [ux, uy, 2]
orn = p.getQuaternionFromEuler([0, 0, 0])
p.changeConstraint(root_robe_c,
pivot,
jointChildFrameOrientation=orn,
maxForce=500)
# manually apply viscous friction: f_damping = -Damping*omega
'''
for j in range(n_joints):
q_state = p.getJointStateMultiDof(rope, j)
omega = q_state[1]
f_dampling = [-Damping*v for v in omega]
p.setJointMotorControlMultiDof(rope, j, p.TORQUE_CONTROL, force=f_dampling)
'''
# update
p.stepSimulation()
import pybullet as p
import time
import random
p.connect(p.GUI)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
colors = [[0.98, 0.41, 0.12], [0.70, 0, 0], [1, 1, 0], [0, 0, 1],
[0, 0.64, 0.37], [1, 1, 1]]
for x in range(-1, 2):
for y in range(-1, 2):
for z in range(-1, 2):
xyz = [x * 2, y * 2, z * 2]
cube = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[0.2, 0.2, 0.2])
cubeMB = p.createMultiBody(1, cube, -1, basePosition=xyz)
cuid = p.createConstraint(cube, -1, -1, -1, p.JOINT_FIXED,
[0, 0, 0], [0, 0, 0], xyz)
color = random.choice(colors)
p.changeVisualShape(cube,
-1,
rgbaColor=[color[0], color[1], color[2], 1])
p.changeConstraint(
cuid, [0, 0, 0],
maxForce=20) #maxForce can only be applied in changeConstraint
while 1:
time.sleep(0.001)
p.stepSimulation()
def changePos(self, new_pos, speed=1):
p.changeConstraint(
self.ball_cid, new_pos, maxForce=25000 * self.scaling * speed)
def _step(self,action):
done = False
aspect = 1
camTargetPos = [0,0,0]
yaw = 40
pitch = 10.0
roll=0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 240
nearPlane = 0.0001
farPlane = 0.022
lightDirection = [0,1,0]
lightColor = [1,1,1]#optional
agent_po = pb.getBasePositionAndOrientation(self.agent_mb)
x = agent_po[0][0]
y = agent_po[0][1]
z = agent_po[0][2]
if action == 0: #down
x += self.move
elif action == 1: #up
x -= self.move
elif action == 2: #left
y += self.move
elif action == 3: #right
y -= self.move
elif action == 4: #<
z += self.move
elif action == 5: #>
z -= self.move
elif action == 9:
print(self.current_observation[:100])
print(np.amax(self.current_observation))
print(np.unique(self.current_observation))
if self.is_touching():
print('I am touching')
else:
print('I am not touching')
pivot = [x,y,z]
orn = pb.getQuaternionFromEuler([0,0,0])
pb.changeConstraint(self.agent_cid,pivot,
jointChildFrameOrientation=[0,1,0,1],
maxForce=100)
viewMatrix = self.ahead_view()
projectionMatrix = pb.computeProjectionMatrixFOV(self.fov,aspect,
nearPlane,farPlane)
w,h,img_arr,depths,mask = pb.getCameraImage(self.cameraImageHeight,
self.cameraImageHeight,
viewMatrix,
projectionMatrix,
lightDirection,
lightColor,
renderer=pb.ER_TINY_RENDERER)
info = [42] #answer to life,TODO use real values
pb.stepSimulation()
self.steps += 1
#reward if moving towards the object or touching the object
reward = 0
if self.is_touching():
touch_reward = 100
new_obs = np.absolute(depths - 1.0)
# if you want binary representation of depth camera, uncomment
# the line below
#new_obs[new_obs > 0] = 1
self.current_observation = new_obs.flatten()
else:
touch_reward = 0
self.current_observation = np.zeros(self.cameraImageHeight*self.cameraImageHeight)
agent_po = pb.getBasePositionAndOrientation(self.agent_mb)
obj_po = pb.getBasePositionAndOrientation(self.obj_to_classify)
distance = math.sqrt(sum([(xi-yi)**2 for xi,yi in zip(obj_po[0],agent_po[0])])) #TODO faster euclidean
if distance < self.prev_distance:
reward += 1 * (self.max_steps - self.steps)
elif distance > self.prev_distance:
reward -= 10
reward -= distance
reward += touch_reward
self.prev_distance = distance
if 'debug' in self.options and self.options['debug'] == True:
print("touch reward ",touch_reward)
print("action ",action)
print("reward ",reward)
print("distance ",distance)
if self.steps >= self.max_steps or self.is_touching():
done = True
return self.current_observation,reward,done,info
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)
]
def add_cable(self,
env,
size_range,
info={},
cable_idx=0,
direction='z',
max_force=100):
"""Add a cable like Andy does it in his cable environment.
Add each `part_id` to (a) env.objects, (b) object_points, (c) _IDs,
and (d) cable_bead_IDs. For (b) it is because, like the sweeping env,
the demonstrator detects the one farthest from the goal to use as the
pick, and `object_points` is also used to tally up items that are
within the zone, to compute the reward, or the net % improvement, so
if we add more cables, all beads in them need to be added to this
dict. But I'm adding (d) so that we can distinguish between bead vs
non-bead objects --- just in case!
When iterating through the number of parts, ensure that the given
cable is _separate_ from prior cables, in case there are more than
one. ALL beads are put in the `env.objects` list.
The zone_range is used because we need the cables to start outside of
the zone. However, we should check if sampling multiple cables will
work; there might not be space to sample a zone and two cables.
Parameters
----------
:size_range: Used to indicate the area of the target, so the beads
avoid spawning there.
:info: Stores relevant stuff, such as for ground-truth targets.
:cable_idx: Only useful if we spawn multiple cables.
:direction: Usually we want z, for verticality.
Returns nothing, but could later return the bead IDs if needed.
"""
num_parts = self.num_parts
radius = self.radius
length = self.length
color = self.colors[cable_idx] + [1]
color_end = U.COLORS['yellow'] + [1]
# Add beaded cable.
distance = length / num_parts
position, _ = self.random_pose(env, size_range)
position = np.float32(position)
part_shape = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[radius] * 3)
part_visual = p.createVisualShape(p.GEOM_SPHERE, radius=radius * 1.5)
# Iterate through parts and create constraints as needed.
for i in range(num_parts):
if direction == 'x':
position[0] += distance
parent_frame = (distance, 0, 0)
elif direction == 'y':
position[1] += distance
parent_frame = (0, distance, 0)
else:
position[2] += distance
parent_frame = (0, 0, distance)
part_id = p.createMultiBody(0.1,
part_shape,
part_visual,
basePosition=position)
if i > 0:
constraint_id = p.createConstraint(
parentBodyUniqueId=env.objects[-1],
parentLinkIndex=-1,
childBodyUniqueId=part_id,
childLinkIndex=-1,
jointType=p.JOINT_POINT2POINT,
jointAxis=(0, 0, 0),
parentFramePosition=parent_frame,
childFramePosition=(0, 0, 0))
p.changeConstraint(constraint_id, maxForce=max_force)
if (i > 0) and (i < num_parts - 1):
p.changeVisualShape(part_id, -1, rgbaColor=color)
elif i == num_parts - 1:
p.changeVisualShape(part_id, -1, rgbaColor=color_end)
# The usual for tracking IDs. Four things to add.
self.cable_bead_IDs.append(part_id)
self._IDs[part_id] = f'cable_part_{str(part_id).zfill(2)}'
env.objects.append(part_id)
self.object_points[part_id] = np.float32((0, 0, 0)).reshape(3, 1)
# Get target placing positions for each cable bead, if applicable.
if self._name == 'cable-shape' or self._name == 'cable-shape-notarget' or \
self._name == 'cable-line-notarget':
# ----------------------------------------------------------- #
# Here, zone_pose = square_pose, unlike Ravens cable, where the
# zone_pose is shifted so that its center matches the straight
# line segment center. For `true_position`, we use `zone_pose`
# but apply the correct offset to deal with the sides. Note
# that `length` is the size of a fully smoothed cable, BUT we
# made a rectangle with each side <= length.
# ----------------------------------------------------------- #
lx = info['lengthx']
ly = info['lengthy']
r = radius
if info['nb_sides'] == 1:
# Here it's just a straight line on the 'lx' side.
x_coord = lx / 2 - (distance * i)
y_coord = 0
true_position = (x_coord - r, y_coord, 0)
elif info['nb_sides'] == 2:
# Start from lx side, go 'left' to the pivot point, then on
# the ly side, go 'upwards' but offset by `i`. For radius
# offset, I just got this by tuning. XD
if i < info['cutoff']:
x_coord = lx / 2 - (distance * i)
y_coord = -ly / 2
true_position = (x_coord - r, y_coord, 0)
else:
x_coord = -lx / 2
y_coord = -ly / 2 + (distance * (i - info['cutoff']))
true_position = (x_coord, y_coord + r, 0)
elif info['nb_sides'] == 3:
# Start from positive lx, positive ly, go down to first
# pivot. Then go left to the second pivot, then up again.
# For v1, division by two is because we assume BOTH of the
# 'ly edges' were divided by two.
v1 = (self.num_parts - info['cutoff']) / 2
v2 = self.num_parts - v1
if i < v1:
x_coord = lx / 2
y_coord = ly / 2 - (distance * i)
true_position = (x_coord, y_coord - r, 0)
elif i < v2:
x_coord = lx / 2 - (distance * (i - v1))
y_coord = -ly / 2
true_position = (x_coord - r, y_coord, 0)
else:
x_coord = -lx / 2
y_coord = -ly / 2 + (distance * (i - v2))
true_position = (x_coord, y_coord + r, 0)
elif info['nb_sides'] == 4:
# I think this is similar to the 2-side case: we start in
# the same direction and go counter-clockwise.
v1 = info['cutoff'] / 2
v2 = num_parts / 2
v3 = (num_parts + info['cutoff']) / 2
if i < v1:
x_coord = lx / 2 - (distance * i)
y_coord = -ly / 2
true_position = (x_coord, y_coord, 0)
elif i < v2:
x_coord = -lx / 2
y_coord = -ly / 2 + (distance * (i - v1))
true_position = (x_coord, y_coord, 0)
elif i < v3:
x_coord = -lx / 2 + (distance * (i - v2))
y_coord = ly / 2
true_position = (x_coord, y_coord, 0)
else:
x_coord = lx / 2
y_coord = ly / 2 - (distance * (i - v3))
true_position = (x_coord, y_coord, 0)
# Map true_position onto the workspace from zone_pose.
true_position = self.apply(self.zone_pose, true_position)
# See `cable.py`: just get the places and steps set.
self.goal['places'][part_id] = (true_position, (0, 0, 0, 1.))
symmetry = 0
self.goal['steps'][0][part_id] = (symmetry, [part_id])
# Debugging target zones.
if self.target_debug_markers:
sq_pose = ((true_position[0], true_position[1], 0.002),
(0, 0, 0, 1))
sq_template = 'assets/square/square-template-allsides-blue.urdf'
replace = {'DIM': (0.003, ), 'HALF': (0.003 / 2, )}
urdf = self.fill_template(sq_template, replace)
env.add_object(urdf, sq_pose, fixed=True)
os.remove(urdf)
else:
print(f'Warning, env {self._name} will not have goals.')
import pybullet as p
import time
import math
p.connect(p.GUI)
p.loadURDF("plane.urdf")
cubeId = p.loadURDF("cube_small.urdf",0,0,1)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1])
print (cid)
print (p.getConstraintUniqueId(0))
prev=[0,0,1]
a=-math.pi
while 1:
a=a+0.01
if (a>math.pi):
a=-math.pi
time.sleep(.01)
p.setGravity(0,0,-10)
pivot=[a,0,1]
orn = p.getQuaternionFromEuler([a,0,0])
p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50)
p.removeConstraint(cid)
def reset_position_orientation(self, pos, orn):
p.changeConstraint(self.cid, pos, orn)
]
ob = objects[0]
jointPositions = [
0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000,
-2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193,
0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517
]
for ji in range(p.getNumJoints(ob)):
p.resetJointState(ob, ji, jointPositions[ji])
p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0)
cid0 = p.createConstraint(1, 3, 1, 6, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid0, maxForce=500.000000)
cid1 = p.createConstraint(1, 16, 1, 19, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid1, maxForce=500.000000)
cid2 = p.createConstraint(1, 9, 1, 12, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid2, maxForce=500.000000)
cid3 = p.createConstraint(1, 22, 1, 25, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid3, maxForce=500.000000)
def go_to_pose(self,
target_pose,
obstacles=[],
attachments=[],
cart_traj=False,
use_policy=False,
maxForce=100):
total_traj = []
if self.pw.handonly:
p.changeConstraint(self.pw.cid,
target_pose[0],
target_pose[1],
maxForce=maxForce)
for i in range(80):
simulate_for_duration(self.dt_pose)
self.pw.steps_taken += self.dt_pose
if self.pw.steps_taken >= self.total_timeout:
return total_traj
ee_loc = p.getBasePositionAndOrientation(self.pw.robot)[0]
distance = np.linalg.norm(np.array(ee_loc) - target_pose[0])
if distance < 1e-3:
break
total_traj.append(ee_loc)
if self.pipe_attach is not None:
self.squeeze(force=self.squeeze_force)
else:
for i in range(50):
end_conf = inverse_kinematics_helper(self.pw.robot,
self.ee_link, target_pose)
if not use_policy:
motion_plan = plan_joint_motion(self.pw.robot,
get_movable_joints(
self.pw.robot),
end_conf,
obstacles=obstacles,
attachments=attachments)
if motion_plan is not None:
for conf in motion_plan:
self.go_to_conf(conf)
ee_loc = p.getLinkState(self.pw.robot, 8)
if cart_traj:
total_traj.append(ee_loc[0] + ee_loc[1])
else:
total_traj.append(conf)
if self.pipe_attach is not None:
self.squeeze(force=self.squeeze_force)
else:
ee_loc = p.getLinkState(self.pw.robot, 8)
next_loc = self.policy.predict(
np.array(ee_loc[0] + ee_loc[1]).reshape(1, 7))[0]
next_pos = next_loc[0:3]
next_quat = next_loc[3:]
next_conf = inverse_kinematics_helper(
self.pw.robot, self.ee_link, (next_pos, next_quat))
if cart_traj:
total_traj.append(next_loc)
else:
total_traj.append(next_conf)
self.go_to_conf(next_conf)
if self.pipe_attach is not None:
self.squeeze(force=self.squeeze_force)
ee_loc = p.getLinkState(self.pw.robot, 8)[0]
distance = np.linalg.norm(np.array(ee_loc) - target_pose[0])
if distance < 1e-3:
break
return total_traj
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)
def __init__(self, assets_root, robot, ee, obj_ids):
"""Creates suction and 'attaches' it to the robot.
Has special cases when dealing with rigid vs deformables. For rigid,
only need to check contact_constraint for any constraint. For soft
bodies (i.e., cloth or bags), use cloth_threshold to check distances
from gripper body (self.body) to any vertex in the cloth mesh. We
need correct code logic to handle gripping potentially a rigid or a
deformable (and similarly for releasing).
To be clear on terminology: 'deformable' here should be interpreted
as a PyBullet 'softBody', which includes cloths and bags. There's
also cables, but those are formed by connecting rigid body beads, so
they can use standard 'rigid body' grasping code.
To get the suction gripper pose, use p.getLinkState(self.body, 0),
and not p.getBasePositionAndOrientation(self.body) as the latter is
about z=0.03m higher and empirically seems worse.
Args:
assets_root: str for root directory with assets.
robot: int representing PyBullet ID of robot.
ee: int representing PyBullet ID of end effector link.
obj_ids: list of PyBullet IDs of all suctionable objects in the env.
"""
super().__init__(assets_root)
# Load suction gripper base model (visual only).
pose = ((0.487, 0.109, 0.438), p.getQuaternionFromEuler((np.pi, 0, 0)))
base = pybullet_utils.load_urdf(
p, os.path.join(self.assets_root, SUCTION_BASE_URDF), pose[0],
pose[1])
p.createConstraint(parentBodyUniqueId=robot,
parentLinkIndex=ee,
childBodyUniqueId=base,
childLinkIndex=-1,
jointType=p.JOINT_FIXED,
jointAxis=(0, 0, 0),
parentFramePosition=(0, 0, 0),
childFramePosition=(0, 0, 0.01))
# Load suction tip model (visual and collision) with compliance.
# urdf = 'assets/ur5/suction/suction-head.urdf'
pose = ((0.487, 0.109, 0.347), p.getQuaternionFromEuler((np.pi, 0, 0)))
self.body = pybullet_utils.load_urdf(
p, os.path.join(self.assets_root, SUCTION_HEAD_URDF), pose[0],
pose[1])
constraint_id = p.createConstraint(parentBodyUniqueId=robot,
parentLinkIndex=ee,
childBodyUniqueId=self.body,
childLinkIndex=-1,
jointType=p.JOINT_FIXED,
jointAxis=(0, 0, 0),
parentFramePosition=(0, 0, 0),
childFramePosition=(0, 0, -0.08))
p.changeConstraint(constraint_id, maxForce=50)
# Reference to object IDs in environment for simulating suction.
self.obj_ids = obj_ids
# Indicates whether gripper is gripping anything (rigid or def).
self.activated = False
# For gripping and releasing rigid objects.
self.contact_constraint = None
# Defaults for deformable parameters, and can override in tasks.
self.def_ignore = 0.035 # TODO(daniel) check if this is needed
self.def_threshold = 0.030
self.def_nb_anchors = 1
# Track which deformable is being gripped (if any), and anchors.
self.def_grip_item = None
self.def_grip_anchors = []
# Determines release when gripped deformable touches a rigid/def.
# TODO(daniel) should check if the code uses this -- not sure?
self.def_min_vetex = None
self.def_min_distance = None
# Determines release when a gripped rigid touches defs (e.g. cloth-cover).
self.init_grip_distance = None
self.init_grip_item = None
def _step(self, action):
done = False
#reward (float): amount of reward achieved by the previous action. The scale varies between environments, but the goal is always to increase your total reward.
#done (boolean): whether it's time to reset the environment again. Most (but not all) tasks are divided up into well-defined episodes, and done being True indicates the episode has terminated. (For example, perhaps the pole tipped too far, or you lost your last life.)
#observation (object): an environment-specific object representing your observation of the environment. For example, pixel data from a camera, joint angles and joint velocities of a robot, or the board state in a board game.
#info (dict): diagnostic information useful for debugging. It can sometimes be useful for learning (for example, it might contain the raw probabilities behind the environment's last state change). However, official evaluations of your agent are not allowed to use this for learning.
def convertSensor(finger_index):
if finger_index == self.indexId:
return random.uniform(-1, 1)
#return 0
else:
#return random.uniform(-1,1)
return 0
def convertAction(action):
#converted = (action-30)/10
#converted = (action-16)/10
if action == 6:
converted = -1
elif action == 25:
converted = 1
#print("action ",action)
#print("converted ",converted)
return converted
aspect = 1
camTargetPos = [0, 0, 0]
yaw = 40
pitch = 10.0
roll = 0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 240
nearPlane = 0.0001
farPlane = 0.022
lightDirection = [0, 1, 0]
lightColor = [1, 1, 1] #optional
fov = 50 #10 or 50
hand_po = pb.getBasePositionAndOrientation(self.agent)
ho = pb.getQuaternionFromEuler([0.0, 0.0, 0.0])
# So when trying to modify the physics of the engine, we run into some problems. If we leave
# angular damping at default (0.04) then the hand rotates when moving up and dow, due to torque.
# If we set angularDamping to 100.0 then the hand will bounce off into the background due to
# all the stored energy, when it makes contact with the object. The below set of parameters seem
# to have a reasonably consistent performance in keeping the hand level and not inducing unwanted
# behavior during contact.
pb.changeDynamics(self.agent, linkIndex=-1, angularDamping=0.9999)
if action == 65298 or action == 0: #down
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0] + self.move, hand_po[0][1], hand_po[0][2]),
ho,
maxForce=self.tinyForce)
elif action == 65297 or action == 1: #up
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0] - self.move, hand_po[0][1], hand_po[0][2]),
ho,
maxForce=self.tinyForce)
elif action == 65295 or action == 2: #left
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0], hand_po[0][1] + self.move, hand_po[0][2]),
ho,
maxForce=self.tinyForce)
elif action == 65296 or action == 3: #right
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0], hand_po[0][1] - self.move, hand_po[0][2]),
ho,
maxForce=self.tinyForce)
elif action == 44 or action == 4: #<
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0], hand_po[0][1], hand_po[0][2] + self.move),
ho,
maxForce=self.tinyForce)
elif action == 46 or action == 5: #>
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0], hand_po[0][1], hand_po[0][2] - self.move),
ho,
maxForce=self.tinyForce)
elif action >= 6 and action <= 7:
# keeps the hand from moving towards origin
pb.changeConstraint(self.hand_cid,
(hand_po[0][0], hand_po[0][1], hand_po[0][2]),
ho,
maxForce=200)
if action == 7:
action = 25 #bad kludge redo all this code
index = convertAction(
action) # action = 6 or 25 due to kludge -> return -1 or 1
# Getting positions of the index joints to use for moving to a relative position
joint17Pos = pb.getJointState(self.agent, 17)[0]
joint19Pos = pb.getJointState(self.agent, 19)[0]
joint21Pos = pb.getJointState(self.agent, 21)[0]
# need to keep the multiplier relatively small otherwise the joint will continue to move
# when you take other actions
finger_jump = 0.1
newJoint17Pos = joint17Pos + index * finger_jump
newJoint19Pos = joint19Pos + index * finger_jump
newJoint21Pos = joint21Pos + index * finger_jump
# following values found by experimentation
if newJoint17Pos <= -0.7:
newJoint17Pos = -0.7
elif newJoint17Pos >= 0.57:
newJoint17Pos = 0.57
if newJoint19Pos <= 0.13:
newJoint19Pos = 0.13
elif newJoint19Pos >= 0.42:
newJoint19Pos = 0.42
if newJoint21Pos <= -0.8:
newJoint21Pos = -0.8
elif newJoint21Pos >= 0.58:
newJoint21Pos = 0.58
pb.setJointMotorControl2(self.agent, 17, pb.POSITION_CONTROL,
newJoint17Pos)
pb.setJointMotorControl2(self.agent, 19, pb.POSITION_CONTROL,
newJoint19Pos)
pb.setJointMotorControl2(self.agent, 21, pb.POSITION_CONTROL,
newJoint21Pos)
if self.downCameraOn: viewMatrix = down_view()
else: viewMatrix = self.ahead_view()
projectionMatrix = pb.computeProjectionMatrixFOV(
fov, aspect, nearPlane, farPlane)
w, h, img_arr, depths, mask = pb.getCameraImage(
200,
200,
viewMatrix,
projectionMatrix,
lightDirection,
lightColor,
renderer=pb.ER_TINY_RENDERER)
#w,h,img_arr,depths,mask = pb.getCameraImage(200,200, viewMatrix,projectionMatrix, lightDirection,lightColor,renderer=pb.ER_BULLET_HARDWARE_OPENGL)
new_obs = np.absolute(depths - 1.0)
new_obs[new_obs > 0] = 1
self.current_observation = new_obs.flatten()
info = [42] #answer to life,TODO use real values
pb.stepSimulation()
self.steps += 1
#reward if moving towards the object or touching the object
reward = 0
max_steps = 1000
if self.is_touching():
touch_reward = 10
if 'debug' in self.options and self.options['debug'] == True:
print("TOUCHING!!!!")
else:
touch_reward = 0
obj_po = pb.getBasePositionAndOrientation(self.obj_to_classify)
distance = math.sqrt(
sum([(xi - yi)**2 for xi, yi in zip(obj_po[0], hand_po[0])
])) #TODO faster euclidean
#distance = np.linalg.norm(obj_po[0],hand_po[0])
#print("distance:",distance)
if distance < self.prev_distance:
reward += 1 * (max_steps - self.steps)
elif distance > self.prev_distance:
reward -= 10
reward -= distance
reward += touch_reward
self.prev_distance = distance
if self.steps >= max_steps or self.is_touching():
done = True
return self.current_observation, reward, done, info
p.connect(p.GUI)
p.loadURDF("plane.urdf", 0, 0, -2)
wheelA = p.loadURDF("differential/diff_ring.urdf", [0, 0, 0])
for i in range(p.getNumJoints(wheelA)):
print(p.getJointInfo(wheelA, i))
p.setJointMotorControl2(wheelA, i, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
c = p.createConstraint(wheelA,
1,
wheelA,
3,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(wheelA,
2,
wheelA,
4,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(wheelA,
1,
wheelA,
4,
def _move_pusher(self, end):
p.changeConstraint(self.gripperConstraint,
end,
maxForce=self.MAX_FORCE)
self._close_gripper()
force=0)
p.getJointInfo(car, wheel)
wheels = [8, 15]
print("----------------")
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,
9,
car,
11,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(car,
10,
car,
13,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,
9,
car,
13,
print(p.getJointInfo(wheelA, i))
p.setJointMotorControl2(wheelA,
i,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
c = p.createConstraint(wheelA,
1,
wheelA,
3,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(wheelA,
2,
wheelA,
4,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(wheelA,
1,
wheelA,
4,
pybullet_util.set_link_damping(robot, link_id.values(), 0., 0.)
# Joint Friction
pybullet_util.set_joint_friction(robot, joint_id, 0.)
# Rolling Joint
c = p.createConstraint(robot,
link_id['l1'],
robot,
link_id['l2'],
jointType=p.JOINT_GEAR,
jointAxis=[0, 0, 1],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000, erp=2)
pybullet_util.draw_link_frame(robot, link_id['l1'], text="proximal")
pybullet_util.draw_link_frame(robot, link_id['l2'], text="distal")
pybullet_util.draw_link_frame(robot, link_id['ee'], text="ee")
# Run Sim
t = 0
count = 0
while (1):
if count % 100:
print("count : ", count)
# Get SensorData
sensor_data = pybullet_util.get_sensor_data(robot, joint_id, link_id,
pos_basejoint_to_basecom,
import pybullet as p
import time
import math
p.connect(p.GUI)
p.loadURDF("plane.urdf")
cubeId = p.loadURDF("cube_small.urdf", 0, 0, 1)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1)
cid = p.createConstraint(cubeId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 1])
print(cid)
print(p.getConstraintUniqueId(0))
prev = [0, 0, 1]
a = -math.pi
while 1:
a = a + 0.01
if (a > math.pi):
a = -math.pi
time.sleep(.01)
p.setGravity(0, 0, -10)
pivot = [a, 0, 1]
orn = p.getQuaternionFromEuler([a, 0, 0])
p.changeConstraint(cid, pivot, jointChildFrameOrientation=orn, maxForce=50)
p.removeConstraint(cid)
"robotiq_85_left_finger_tip_joint",
"robotiq_85_right_inner_knuckle_joint",
"robotiq_85_right_finger_tip_joint"]
mimicMul = [-1,-1,-1,-1,-1,-1]
mimicChildList = []
parent = joints[mimicParentName]
constraints = dict()
for i, name in enumerate(mimicChildName):
child = joints[name]
c = p.createConstraint(robotID, parent.id,
robotID, child.id,
jointType=p.JOINT_GEAR,
jointAxis=[0,0,1],
parentFramePosition=[0,0,0],
childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=mimicMul[i], maxForce=child.maxForce)
constraints[name] = c
# start simulation
try:
flag = True
userParams = dict()
for name in controlJoints:
joint = joints[name]
userParam = p.addUserDebugParameter(name, joint.lowerLimit, joint.upperLimit, 0)
userParams[name] = userParam
while(flag):
for name in controlJoints:
joint = joints[name]
pose = p.readUserDebugParameter(userParams[name])
p.setJointMotorControl2(robotID, joint.id, p.POSITION_CONTROL,
p.resetJointState(quadruped, motor_front_rightL_joint, motordir[4] * halfpi)
p.resetJointState(quadruped, knee_front_rightL_link, motordir[4] * kneeangle)
p.resetJointState(quadruped, motor_front_rightR_joint, motordir[5] * halfpi)
p.resetJointState(quadruped, knee_front_rightR_link, motordir[5] * kneeangle)
p.resetJointState(quadruped, motor_back_rightL_joint, motordir[6] * halfpi)
p.resetJointState(quadruped, knee_back_rightL_link, motordir[6] * kneeangle)
p.resetJointState(quadruped, motor_back_rightR_joint, motordir[7] * halfpi)
p.resetJointState(quadruped, knee_back_rightR_link, motordir[7] * kneeangle)
#p.getNumJoints(1)
if (toeConstraint):
cid = p.createConstraint(quadruped, knee_front_leftR_link, quadruped, knee_front_leftL_link,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_front_rightR_link, quadruped, knee_front_rightL_link,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_back_leftR_link, quadruped, knee_back_leftL_link,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_back_rightR_link, quadruped, knee_back_rightL_link,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
if (1):
p.setJointMotorControl(quadruped, knee_front_leftL_link, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_front_leftR_link, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
def reset(self, env):
super().reset(env)
n_parts = 20
radius = 0.005
length = 2 * radius * n_parts * np.sqrt(2)
# Add 3-sided square.
square_size = (length, length, 0)
square_pose = self.get_random_pose(env, square_size)
square_template = 'square/square-template.urdf'
replace = {'DIM': (length,), 'HALF': (length / 2 - 0.005,)}
urdf = self.fill_template(square_template, replace)
env.add_object(urdf, square_pose, 'fixed')
os.remove(urdf)
# Get corner points of square.
corner0 = (length / 2, length / 2, 0.001)
corner1 = (-length / 2, length / 2, 0.001)
corner0 = utils.apply(square_pose, corner0)
corner1 = utils.apply(square_pose, corner1)
# Add cable (series of articulated small blocks).
increment = (np.float32(corner1) - np.float32(corner0)) / n_parts
position, _ = self.get_random_pose(env, (0.1, 0.1, 0.1))
position = np.float32(position)
part_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=[radius] * 3)
part_visual = p.createVisualShape(p.GEOM_SPHERE, radius=radius * 1.5)
parent_id = -1
targets = []
objects = []
for i in range(n_parts):
position[2] += np.linalg.norm(increment)
part_id = p.createMultiBody(0.1, part_shape, part_visual,
basePosition=position)
if parent_id > -1:
constraint_id = p.createConstraint(
parentBodyUniqueId=parent_id,
parentLinkIndex=-1,
childBodyUniqueId=part_id,
childLinkIndex=-1,
jointType=p.JOINT_POINT2POINT,
jointAxis=(0, 0, 0),
parentFramePosition=(0, 0, np.linalg.norm(increment)),
childFramePosition=(0, 0, 0))
p.changeConstraint(constraint_id, maxForce=100)
if (i > 0) and (i < n_parts - 1):
color = utils.COLORS['red'] + [1]
p.changeVisualShape(part_id, -1, rgbaColor=color)
env.obj_ids['rigid'].append(part_id)
parent_id = part_id
target_xyz = np.float32(corner0) + i * increment + increment / 2
objects.append((part_id, (0, None)))
targets.append((target_xyz, (0, 0, 0, 1)))
matches = np.clip(np.eye(n_parts) + np.eye(n_parts)[::-1], 0, 1)
self.goals.append((objects, matches, targets,
False, False, 'pose', None, 1))
for i in range(480):
p.stepSimulation()
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]
motordir[5] * kneeangle)
p.resetJointState(quadruped, motor_back_rightL_joint, motordir[6] * halfpi)
p.resetJointState(quadruped, knee_back_rightL_link,
motordir[6] * kneeangle)
p.resetJointState(quadruped, motor_back_rightR_joint, motordir[7] * halfpi)
p.resetJointState(quadruped, knee_back_rightR_link,
motordir[7] * kneeangle)
#p.getNumJoints(1)
if (toeConstraint):
cid = p.createConstraint(quadruped, knee_front_leftR_link, quadruped,
knee_front_leftL_link, p.JOINT_POINT2POINT,
[0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_front_rightR_link, quadruped,
knee_front_rightL_link, p.JOINT_POINT2POINT,
[0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_back_leftR_link, quadruped,
knee_back_leftL_link, p.JOINT_POINT2POINT,
[0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_back_rightR_link, quadruped,
knee_back_rightL_link, p.JOINT_POINT2POINT,
[0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
if (1):
p.setJointMotorControl(quadruped, knee_front_leftL_link,
p.loadURDF("plane.urdf")
#p.loadSDF("stadium.sdf")
car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True)
for i in range (p.getNumJoints(car)):
print (p.getJointInfo(car,i))
for wheel in range(p.getNumJoints(car)):
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.getJointInfo(car,wheel)
wheels = [8,15]
print("----------------")
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
def load_urdf(self):
"""Load the URDF of the racecar into the environment
The racecar URDF comes with its own dimensions and
textures, collidables.
"""
# Load robot
self.car = p.loadURDF(Utilities.gen_urdf_path("racecar/racecar_differential.urdf"), [0, 0, 0.5], useFixedBase=False, globalScaling=0.5)
for wheel in range(p.getNumJoints(self.car)):
p.setJointMotorControl2(self.car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
p.getJointInfo(self.car, wheel)
# Constraints
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(self.car,
9,
self.car,
11,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(self.car,
10,
self.car,
13,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(self.car,
9,
self.car,
13,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(self.car,
16,
self.car,
18,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(self.car,
16,
self.car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(self.car,
17,
self.car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(self.car,
1,
self.car,
18,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
c = p.createConstraint(self.car,
3,
self.car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
POSITION = 1
ORIENTATION = 2
BUTTONS = 6
p.setRealTimeSimulation(1)
controllerId = -1
while True:
events = p.getVREvents()
for e in (events):
#print (e[BUTTONS])
if (e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED):
if (controllerId >= 0):
controllerId = -1
else:
controllerId = e[0]
if (e[0] == controllerId):
if (robot_cid >= 0):
p.changeConstraint(robot_cid, e[POSITION], e[ORIENTATION], maxForce=5000)
if (e[BUTTONS][32] & p.VR_BUTTON_WAS_TRIGGERED):
if (robot_cid >= 0):
p.removeConstraint(robot_cid)
#q = [0,0,0,1]
euler = p.getEulerFromQuaternion(e[ORIENTATION])
q = p.getQuaternionFromEuler([euler[0], euler[1], 0]) #-euler[0],-euler[1],-euler[2]])
robot_cid = p.createConstraint(minitaur, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0.1, 0, 0],
e[POSITION], q, e[ORIENTATION])
motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint'] hip_leftR_link = jointNameToId['hip_leftR_link'] knee_back_leftR_link = jointNameToId['knee_back_leftR_link'] motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint'] motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] motordir=[-1,-1,-1,-1,1,1,1,1] halfpi = 1.57079632679 kneeangle = -2.1834 p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi) p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle) p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi) p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle) cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) p.changeConstraint(cid,maxForce=10000) p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi) p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle) p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi) p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle) cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) p.changeConstraint(cid,maxForce=10000) p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) #p.getNumJoints(1)
portname = "/dev/cu.usbmodem14"+str(portindex)
print(portname)
ser = getSerialOrNone(portname)
if (ser is not None):
print("COnnected!")
portindex = portindex+1
p.saveWorld("setupTrackerWorld.py")
if (ser.isOpen()):
while True:
events = p.getVREvents(deviceTypeFilter=p.VR_DEVICE_GENERIC_TRACKER)
for e in (events):
p.changeConstraint(hand_cid,e[POSITION],e[ORIENTATION], maxForce=50)
serialSteps = 0
while ser.inWaiting() > 0:
serialSteps=serialSteps+1
if (serialSteps>serialStepsUntilCheckVREvents):
ser.flushInput()
break
line = str(ser.readline())
words = line.split(",")
if (len(words)==6):
pink = convertSensor(words[1])
middle = convertSensor(words[2])
index = convertSensor(words[3])
thumb = convertSensor(words[4])+0.2
def reset(self, ret_images=False, ret_dict=False):
self.setup_timing()
self.task_success = 0
self.human, self.wheelchair, self.robot, self.robot_lower_limits, self.robot_upper_limits, self.human_lower_limits, self.human_upper_limits, self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices, self.gender = self.world_creation.create_new_world(
furniture_type='wheelchair',
static_human_base=True,
human_impairment='random',
print_joints=False,
gender='random')
self.robot_lower_limits = self.robot_lower_limits[
self.robot_right_arm_joint_indices]
self.robot_upper_limits = self.robot_upper_limits[
self.robot_right_arm_joint_indices]
self.reset_robot_joints()
#
# if self.robot_type == 'jaco':
# wheelchair_pos, wheelchair_orient = p.getBasePositionAndOrientation(self.wheelchair,
# physicsClientId=self.id)
# p.resetBasePositionAndOrientation(self.robot, np.array(wheelchair_pos) + np.array([-0.35, -0.3, 0.3]),
# p.getQuaternionFromEuler([0, 0, -np.pi / 2.0], physicsClientId=self.id),
# physicsClientId=self.id)
# base_pos, base_orient = p.getBasePositionAndOrientation(self.robot, physicsClientId=self.id)
joints_positions = [(6, np.deg2rad(-90)), (16, np.deg2rad(-90)),
(28, np.deg2rad(-90)), (31, np.deg2rad(80)),
(35, np.deg2rad(-90)), (38, np.deg2rad(80))]
joints_positions += [
(21, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30))),
(22, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30))),
(23, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30)))
]
self.human_controllable_joint_indices = [20, 21, 22, 23]
self.world_creation.setup_human_joints(
self.human,
joints_positions,
self.human_controllable_joint_indices if
(self.human_control
or self.world_creation.human_impairment == 'tremor') else [],
use_static_joints=True,
human_reactive_force=None)
p.resetBasePositionAndOrientation(
self.human, [0, 0.03, 0.89 if self.gender == 'male' else 0.86],
[0, 0, 0, 1],
physicsClientId=self.id)
human_joint_states = p.getJointStates(
self.human,
jointIndices=self.human_controllable_joint_indices,
physicsClientId=self.id)
self.target_human_joint_positions = np.array(
[x[0] for x in human_joint_states])
self.human_lower_limits = self.human_lower_limits[
self.human_controllable_joint_indices]
self.human_upper_limits = self.human_upper_limits[
self.human_controllable_joint_indices]
# Place a bowl of food on a table
self.table = p.loadURDF(os.path.join(self.world_creation.directory,
'table', 'table_tall.urdf'),
basePosition=[0.35, -0.9, 0],
baseOrientation=p.getQuaternionFromEuler(
[0, 0, 0], physicsClientId=self.id),
physicsClientId=self.id)
# MOUTH SIM
self.mouth_pos = [-0, -0.15, 1.2]
self.mouth_orient = p.getQuaternionFromEuler(
[np.pi / 2, np.pi / 2, -np.pi / 2], physicsClientId=self.id)
self.mouth = p.loadURDF(os.path.join(self.world_creation.directory,
'mouth', 'hole.urdf'),
useFixedBase=True,
basePosition=self.mouth_pos,
baseOrientation=self.mouth_orient,
flags=p.URDF_USE_SELF_COLLISION,
physicsClientId=self.id)
sph_vis = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=0.005,
rgbaColor=[0, 1, 0, 1],
physicsClientId=self.id)
self.mouth_center_vis = p.createMultiBody(baseMass=0.0,
baseCollisionShapeIndex=-1,
baseVisualShapeIndex=sph_vis,
basePosition=self.mouth_pos,
useMaximalCoordinates=False,
physicsClientId=self.id)
self.batch_ray_offsets = [[0.08, 0, 0], [-0.08, 0, 0], [0, 0, 0.05],
[0, 0, -0.05]]
self.ray_markers = [
p.createMultiBody(baseMass=0.0,
baseCollisionShapeIndex=-1,
baseVisualShapeIndex=sph_vis,
basePosition=self.mouth_pos,
useMaximalCoordinates=False,
physicsClientId=self.id) for i in range(4)
]
self.ray_markers_end = [
p.createMultiBody(baseMass=0.0,
baseCollisionShapeIndex=-1,
baseVisualShapeIndex=sph_vis,
basePosition=self.mouth_pos,
useMaximalCoordinates=False,
physicsClientId=self.id) for i in range(4)
]
# SCALE = 0.1
# self.mouthVisualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
# fileName=os.path.join(self.world_creation.directory, 'mouth', 'hole2.obj'),
# rgbaColor=[0.8, 0.4, 0, 1],
# # specularColor=[0.4, .4, 0],
# visualFramePosition=[0, 0, 0],
# meshScale=[SCALE] * 3,
# physicsClientId=self.id)
# self.mouthCollisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
# fileName=os.path.join(self.world_creation.directory, 'mouth', 'hole2.obj'),
# collisionFramePosition=[0, 0, 0],
# meshScale=[SCALE] * 3,
# flags=p.GEOM_FORCE_CONCAVE_TRIMESH,
# physicsClientId=self.id)
# self.mouth = p.createMultiBody(baseMass=0.1,
# baseInertialFramePosition=[0, 0, 0],
# baseCollisionShapeIndex=self.mouthCollisionShapeId,
# baseVisualShapeIndex=self.mouthVisualShapeId,
# basePosition=[0, 0, 0],
# useMaximalCoordinates=True,
# flags=p.URDF_USE_SELF_COLLISION,
# physicsClientId=self.id)
p.resetDebugVisualizerCamera(cameraDistance=1.10,
cameraYaw=40,
cameraPitch=-45,
cameraTargetPosition=[-0.2, 0, 0.75],
physicsClientId=self.id)
# ROBOT STUFF
# target_pos = np.array(bowl_pos) + np.array([0, -0.1, 0.4]) + self.np_random.uniform(-0.05, 0.05, size=3)
if self.robot_type == 'panda':
# target_pos = [0.2, -0.7, 1.4]
# target_orient = [-0.7350878727462702, -8.032928869253401e-09, 7.4087721728719465e-09,
# 0.6779718425874067]
# target_orient = [0.4902888273008801, -0.46462044731135954, -0.5293302738768326, -0.5133752690981496]
# target_orient = p.getQuaternionFromEuler(np.array([0, 0, np.pi / 2.0]), physicsClientId=self.id)
# self.util.ik_random_restarts(self.robot, 8, target_pos, target_orient, self.world_creation,
# self.robot_right_arm_joint_indices, self.robot_lower_limits,
# self.robot_upper_limits,
# ik_indices=range(len(self.robot_right_arm_joint_indices)), max_iterations=1000,
# max_ik_random_restarts=40, random_restart_threshold=0.01, step_sim=True,
# check_env_collisions=True)
#
# positions = [0.42092164, -0.92326318, -0.33538581, -2.65185322, 1.40763901, 1.81818155, 0.58610855, 0, 0,
# 0.02, 0.02] # 11
# positions = [0.4721324, -0.87257245, -0.2123101, -2.44757844, 1.47352227, 1.90545005, 0.75139663]
positions = [
0.44594466, -0.58616227, -0.78082232, -2.59866731, 1.15649738,
1.54176148, 0.15145287
]
for idx in range(len(positions)):
p.resetJointState(self.robot, idx, positions[idx])
self.world_creation.set_gripper_open_position(self.robot,
position=0.00,
left=False,
set_instantly=True)
self.drop_fork = self.world_creation.init_tool(
self.robot,
mesh_scale=[0.08] * 3,
pos_offset=[0, 0, 0.08], # fork: [0, -0.02, 0.16],
orient_offset=p.getQuaternionFromEuler(
[-0, -np.pi, 0], physicsClientId=self.id),
left=False,
maximal=False)
else:
raise NotImplementedError
# LOAD FOOD ITEM
self.food_type = np.random.randint(0, len(self.foods), dtype=int)
self.food_scale = np.random.uniform(
self.foods_scale_range[self.food_type][0],
self.foods_scale_range[self.food_type][1])
print("LOADING food file: %s with scale %.2f" %
(self.foods[self.food_type], self.food_scale))
self.child_offset = self.food_scale * self.food_offsets[
self.food_type] # [-0.004, -0.0065]
self.food_orient_eul = np.random.rand(3) * 2 * np.pi
self.food_orient_quat = p.getQuaternionFromEuler(
self.food_orient_eul, physicsClientId=self.id)
mesh_scale = np.array(
[self.food_base_scale[self.food_type]] * 3) * self.food_scale
food_visual = p.createVisualShape(
shapeType=p.GEOM_MESH,
fileName=os.path.join(self.world_creation.directory, 'food_items',
self.foods[self.food_type] + ".obj"),
rgbaColor=[1.0, 1.0, 1.0, 1.0],
meshScale=mesh_scale,
physicsClientId=self.id)
food_collision = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=os.path.join(self.world_creation.directory, 'food_items',
self.foods[self.food_type] + "_vhacd.obj"),
meshScale=mesh_scale,
physicsClientId=self.id)
self.food_item = p.createMultiBody(
baseMass=0.012,
baseCollisionShapeIndex=food_collision,
baseVisualShapeIndex=food_visual,
basePosition=[0, 0, 0],
useMaximalCoordinates=False,
physicsClientId=self.id)
p.changeVisualShape(
self.food_item,
-1,
textureUniqueId=p.loadTexture(
os.path.join(self.world_creation.directory, 'food_items',
self.foods[self.food_type] + ".png")))
# Disable collisions between the tool and food item
for ti in list(
range(p.getNumJoints(self.drop_fork,
physicsClientId=self.id))) + [-1]:
for tj in list(
range(
p.getNumJoints(self.food_item,
physicsClientId=self.id))) + [-1]:
p.setCollisionFilterPair(self.drop_fork,
self.food_item,
ti,
tj,
False,
physicsClientId=self.id)
# Create constraint that keeps the food item in the tool
constraint = p.createConstraint(
self.drop_fork,
-1,
self.food_item,
-1,
p.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, -0.025],
childFramePosition=self.child_offset,
parentFrameOrientation=self.food_orient_quat,
childFrameOrientation=[0, 0, 0, 1],
physicsClientId=self.id)
p.changeConstraint(constraint, maxForce=500, physicsClientId=self.id)
# p.resetBasePositionAndOrientation(self.bowl, bowl_pos, p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id)
p.setGravity(0, 0, -9.81, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.robot, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.human, physicsClientId=self.id)
p.setPhysicsEngineParameter(numSubSteps=5,
numSolverIterations=10,
physicsClientId=self.id)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self.id)
# init_pos = [0.0, 0.0, 0.0, -2 * np.pi / 4, 0.0, np.pi / 2, np.pi / 4, 0.0, 0.0, 0.05, 0.05]
# self._reset_robot(init_pos)
# initialize images
for i in range(10):
p.stepSimulation()
ee_pos, ee_orient = p.getBasePositionAndOrientation(self.food_item)
self.viewMat1 = p.computeViewMatrixFromYawPitchRoll(
ee_pos, self.camdist, self.yaw, self.pitch, self.roll, 2, self.id)
self.viewMat2 = p.computeViewMatrixFromYawPitchRoll(
ee_pos, self.camdist, -self.yaw, self.pitch, self.roll, 2, self.id)
self.projMat = p.computeProjectionMatrixFOV(
self.fov, self.img_width / self.img_height, self.near, self.far)
images1 = p.getCameraImage(self.img_width,
self.img_height,
self.viewMat1,
self.projMat,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
physicsClientId=self.id)
images2 = p.getCameraImage(self.img_width,
self.img_height,
self.viewMat2,
self.projMat,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
physicsClientId=self.id)
self.rgb_opengl1 = np.reshape(
images1[2], (self.img_width, self.img_height, 4)) * 1. / 255.
depth_buffer_opengl = np.reshape(images1[3],
[self.img_width, self.img_height])
self.depth_opengl1 = self.far * self.near / (
self.far - (self.far - self.near) * depth_buffer_opengl)
self.rgb_opengl2 = np.reshape(
images2[2], (self.img_width, self.img_height, 4)) * 1. / 255.
depth_buffer_opengl = np.reshape(images2[3],
[self.img_width, self.img_height])
self.depth_opengl2 = self.far * self.near / (
self.far - (self.far - self.near) * depth_buffer_opengl)
if self.gui:
# TODO: do we need to update this every step? prob not
self.im1.set_data(self.rgb_opengl1)
self.im2.set_data(self.rgb_opengl2)
self.im1_d.set_data(self.depth_opengl1)
self.im2_d.set_data(self.depth_opengl2)
print(np.min(self.depth_opengl1), np.max(self.depth_opengl1))
self.fig.canvas.draw()
return self._get_obs(ret_images=ret_images, ret_dict=ret_dict)
