def debug(self):
# pause simulation by space key event
keys = pybullet.getKeyboardEvents()
space_key = ord(' ')
if space_key in keys and keys[space_key] & pybullet.KEY_WAS_TRIGGERED:
print("*" * 100 + "Simulation Paused! Press 'Space' to resume!" +
"*" * 100)
while True:
keys = pybullet.getKeyboardEvents()
if space_key in keys and keys[
space_key] & pybullet.KEY_WAS_TRIGGERED:
break
def update_keys():
global grip
keys = p.getKeyboardEvents()
result = np.array([0, 0, 0])
for k in keys:
if k == ord('l'):
result = np.array([0.005, 0, 0])
elif k == ord('j'):
result = np.array([-0.005, 0, 0])
elif k == ord('i'):
result = np.array([0, 0.005, 0])
elif k == ord('k'):
result = np.array([0, -0.005, 0])
elif k == ord('o'):
result = np.array([0, 0, 0.005])
elif k == ord('u'):
result = np.array([0, 0, -0.005])
elif k == ord('u'):
result = np.array([0, 0, -0.005])
if k == ord('b'):
grip = 1
elif k == ord('n'):
grip = 0
return result
def moveKeyboard(x1, y1, o1, object_list):
"""
Move robot based on keyboard inputs
"""
flag = False
delz = 0
keys = p.getKeyboardEvents()
if ord(b'm') in keys:
if 65297 in keys:
x1 += math.cos(o1) * 0.001
y1 += math.sin(o1) * 0.001
flag = True
if 65298 in keys:
x1 -= math.cos(o1) * 0.001
y1 -= math.sin(o1) * 0.001
flag = True
if ord(b'o') in keys:
delz = 0.001
flag = True
if ord(b'l') in keys:
delz = -0.001
flag = True
if 65295 in keys:
o1 += 0.005
flag = True
if 65296 in keys:
o1 -= 0.005
flag = True
q = p.getQuaternionFromEuler((0, 0, o1))
for obj_id in object_list:
(x, y, z1) = p.getBasePositionAndOrientation(obj_id)[0]
z1 = max(0, z1 + delz)
if p.getBasePositionAndOrientation(obj_id)[0] != ((x1, y1, z1), (q)):
p.resetBasePositionAndOrientation(obj_id, [x1, y1, z1], q)
return x1, y1, o1, flag
def IL_get_action(self):
keys = p.getKeyboardEvents()
forward = 0
turn = 0
# while forward!=0 and turn!=0
# while keys == {}:
# keys = p.getKeyboardEvents()
# print('Empty')
for k,v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_TRIGGERED)):
turn = -0.5
if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_RELEASED)):
turn = 0
if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_TRIGGERED)):
turn = 0.5
if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_RELEASED)):
turn = 0
if (k == p.B3G_UP_ARROW and (v&p.KEY_WAS_TRIGGERED)):
forward=1
if (k == p.B3G_UP_ARROW and (v&p.KEY_WAS_RELEASED)):
forward=0
if (k == p.B3G_DOWN_ARROW and (v&p.KEY_WAS_TRIGGERED)):
forward=-1
if (k == p.B3G_DOWN_ARROW and (v&p.KEY_WAS_RELEASED)):
forward=0
return np.array([forward,turn])
def test_mode(self, test_key, func):
keys = p.getKeyboardEvents()
if len(keys)>0:
for k,v in keys.items():
if v & p.KEY_WAS_TRIGGERED:
if (k==ord(test_key)):
func()
def step(self):
# get keyboard events if gui is active
single_step = False
if self.gui:
# rest if R-key was pressed
rKey = ord('r')
nKey = ord('n')
sKey = ord('s')
tKey = ord('t')
spaceKey = p.B3G_SPACE
keys = p.getKeyboardEvents()
if rKey in keys and keys[rKey] & p.KEY_WAS_TRIGGERED:
self.reset()
if spaceKey in keys and keys[spaceKey] & p.KEY_WAS_TRIGGERED:
self.paused = not self.paused
if sKey in keys and keys[sKey] & p.KEY_IS_DOWN:
single_step = True
if nKey in keys and keys[nKey] & p.KEY_WAS_TRIGGERED:
self.set_gravity(not self.gravity)
if tKey in keys and keys[tKey] & p.KEY_WAS_TRIGGERED:
self.real_time = not self.real_time
p.setRealTimeSimulation(self.real_time)
# check if simulation should continue currently
if not self.paused or single_step:
self.time += self.timestep
p.stepSimulation()
for name, ps in self.pressure_sensors.items():
ps.filter_step()
def step(self, action):
if self.manual_mode:
keys = p.getKeyboardEvents()
m_steering = 0.0
for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
m_steering = -.2
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
m_steering = 0.0
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
m_steering = .2
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
m_steering = 0.0
else:
m_steering = 0.0
self._assign_throttle(action, m_steering)
p.stepSimulation()
self._observation = self._compute_observation()
reward = self._compute_reward()
done = self._compute_done()
self._envStepCounter += 1
return np.array(self._observation), reward, done, {}
def user_control_demo():
ycb_models = YCBModels(
os.path.join('./data/ycb', '**', 'textured-decmp.obj'), )
env = ClutteredPushGrasp(ycb_models,
vis=True,
num_objs=5,
gripper_type='85')
p.resetDebugVisualizerCamera(2.0, -270., -60., (0., 0., 0.))
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1) # Shadows on/off
p.addUserDebugLine([0, -0.5, 0], [0, -0.5, 1.1], [0, 1, 0])
env.reset()
while True:
env.step(None, None, None, True)
# key control
keys = p.getKeyboardEvents()
# key "Z" is down and hold
if (122 in keys) and (keys[122] == 3):
print('Grasping...')
if env.close_gripper(check_contact=True):
print('Grasped!')
# key R
if 114 in keys:
env.open_gripper()
def forward(self, action, dt):
events = p.getKeyboardEvents()
x_speed = np.array([-0.5, 0. , 0. ])
y_speed = np.array([ 0. , 0.5, 0. ])
z_speed = np.array([ 0. , 0. , 0.5])
control = np.array([ 0. , 0. , 0. ])
if p.B3G_LEFT_ARROW in events and events[p.B3G_LEFT_ARROW] == p.KEY_IS_DOWN:
control += x_speed
if p.B3G_RIGHT_ARROW in events and events[p.B3G_RIGHT_ARROW] == p.KEY_IS_DOWN:
control -= x_speed
if p.B3G_UP_ARROW in events and events[p.B3G_UP_ARROW] == p.KEY_IS_DOWN:
control += y_speed
if p.B3G_DOWN_ARROW in events and events[p.B3G_DOWN_ARROW] == p.KEY_IS_DOWN:
control -= y_speed
if p.B3G_SHIFT in events and events[p.B3G_SHIFT] == p.KEY_IS_DOWN:
control += z_speed
if p.B3G_CONTROL in events and events[p.B3G_CONTROL] == p.KEY_IS_DOWN:
control -= z_speed
control = np.maximum(control, -1)
control = np.minimum(control, 1)
self.vel = np.vstack(control)
currentPosition = self.pos
newPosition = [currentPosition[0] + self.vel[0] * dt,
currentPosition[1] + self.vel[1] * dt,
currentPosition[2] + self.vel[2] * dt]
ori = [0,0,0,1]
p.resetBasePositionAndOrientation(self.model_uid, newPosition, ori)
self.broadcast = action["broadcast"] if "broadcast" in action.keys() else {}
def changeCam(self):
cubePos, cubeOrn = p.getBasePositionAndOrientation(self.robot)
a = p.getDebugVisualizerCamera()
cyaw = a[8]
cpitch = a[9]
cdist = a[10]
cubePos = a[11]
keys = p.getKeyboardEvents()
#Keys to change camera
if keys.get(100): #D
cyaw += 0.1
if keys.get(97): #A
cyaw -= 0.1
if keys.get(99): #C
cpitch += 0.1
if keys.get(102): #F
cpitch -= 0.1
if keys.get(122): #Z
cdist += .01
if keys.get(120): #X
cdist -= .01
p.resetDebugVisualizerCamera(cameraDistance=cdist,
cameraYaw=cyaw,
cameraPitch=cpitch,
cameraTargetPosition=cubePos)
def get_key_pressed(self, relevant=None):
pressed_keys = []
events = p.getKeyboardEvents()
key_codes = events.keys()
for key in key_codes:
pressed_keys.append(key)
return pressed_keys
def check_keyboard():
global left_position
global left_orientation
global right_position
global right_orientation
keys = pybullet.getKeyboardEvents()
# Quit
if is_key_pressed(keys, 'q'):
return True
# Left position
if is_key_pressed(keys, 'w'):
left_position[0] += POSITION_STEP
if is_key_pressed(keys, 'e'):
left_position[1] += POSITION_STEP
if is_key_pressed(keys, 'r'):
left_position[2] += POSITION_STEP
# Left rotation
if is_key_pressed(keys, 's'):
left_orientation[0] += ROTATION_STEP
if is_key_pressed(keys, 'd'):
left_orientation[0] += ROTATION_STEP
if is_key_pressed(keys, 'f'):
left_orientation[0] += ROTATION_STEP
return False
def GetKeyEvents(self):
pressed = []
keyEvents = p.getKeyboardEvents()
for char in self.keys:
key = ord(char)
if key in keyEvents and keyEvents[key] & p.KEY_WAS_TRIGGERED:
pressed.append(char)
return pressed
def step(self):
# get keyboard events if gui is active
single_step = False
if self.gui:
# reset if R-key was pressed
rKey = ord('r')
# gravity
nKey = ord('n')
# single step
sKey = ord('s')
# real time
tKey = ord('t')
# randomize terrain
fKey = ord('f')
# pause
spaceKey = p.B3G_SPACE
keys = p.getKeyboardEvents()
if rKey in keys and keys[rKey] & p.KEY_WAS_TRIGGERED:
self.reset()
if spaceKey in keys and keys[spaceKey] & p.KEY_WAS_TRIGGERED:
self.paused = not self.paused
if sKey in keys and keys[sKey] & p.KEY_IS_DOWN:
single_step = True
if nKey in keys and keys[nKey] & p.KEY_WAS_TRIGGERED:
self.set_gravity(not self.gravity)
if tKey in keys and keys[tKey] & p.KEY_WAS_TRIGGERED:
self.real_time = not self.real_time
p.setRealTimeSimulation(self.real_time)
if fKey in keys and keys[fKey] & p.KEY_WAS_TRIGGERED:
# generate new terain
self.terrain.randomize()
# block until pause is over
while self.paused and not single_step:
keys = p.getKeyboardEvents()
if spaceKey in keys and keys[spaceKey] & p.KEY_WAS_TRIGGERED:
self.paused = not self.paused
if sKey in keys and keys[sKey] & p.KEY_IS_DOWN:
single_step = True
# sleep a bit to not block a whole CPU core while waiting
sleep(0.01)
self.time += self.timestep
p.stepSimulation()
for name, ps in self.pressure_sensors.items():
ps.filter_step()
def IsPressed(self):
keys = p.getKeyboardEvents()
if keys.get(65297):
self.up_down -= 0.1
if self.up_down < -4:
self.up_down = -4
# return self.up
if keys.get(65298):
self.up_down += 0.1
if self.up_down > 4:
self.up_down = 4
# return self.down
if keys.get(65296):
self.right_left += 0.1
if self.right_left > 4:
self.right_left = 4
print("R:", self.right_left)
# return self.right
if keys.get(65295):
self.right_left -= 0.1
if self.right_left < -4:
self.right_left = -4
print("L:", self.right_left)
# return self.left
if keys.get(97):
self.rig_lef += 0.1
if self.rig_lef > 4:
self.rig_lef = 4
if keys.get(100):
self.up_dow += 0.1
if self.up_dow > 4:
self.up_dow = 4
if keys.get(111):
self.up_dow -= 0.1
if self.up_dow < -4:
self.up_dow = -4
if keys.get(116):
self.rig_lef -= 0.1
if self.rig_lef < -4:
self.rig_lef = -4
# return pad(self.up, self.down, self.right,self.left)
return self.right_left, self.up_down, self.up_dow, self.rig_lef
def main():
physicsClient = p.connect(p.GUI)
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
p.setGravity(0, 0, -10)
#data path to search object meshes
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf")
clothId = p.loadSoftBody(fileName=os.path.join(assets_path, 'tshirt.obj'),
basePosition=[0, 0, 1.35],
baseOrientation=[0, 0, 0.7071068, 0.7071068],
scale=.9,
collisionMargin=0.02,
useMassSpring=1,
mass=1,
springElasticStiffness=10,
springDampingStiffness=0.2,
useBendingSprings=1,
useFaceContact=1)
# clothId = p.loadSoftBody(fileName=os.path.join(assets_path, 'tshirt_mia.obj'), basePosition=[0.02, 0, -0.05], baseOrientation=[ 0.5, 0.5, 0.5, 0.5 ], scale=.35,
# collisionMargin=0.05, useMassSpring=1, mass=1, springElasticStiffness=8, springDampingStiffness=0.2, useBendingSprings=1)
# cloth_mesh = p.getMeshData(clothId)
# print(cloth_mesh[0], len(cloth_mesh[1]))
# clothId = p.loadSoftBody(fileName=os.path.join(assets_path, 'hospitalgown_adaptivereduce.obj'), basePosition=[0, 0, 2.35], baseOrientation=[ 0, 0, 0.7071068, 0.7071068 ], scale=.9,
# collisionMargin=0.02, useMassSpring=1, mass=1, springElasticStiffness=10, springDampingStiffness=0.2, useBendingSprings=1, useFaceContact=1)
humanoid = MyHumanoid(p)
humanoid.fixBase()
runSimulation = False
useRealTimeSimulation = 0
cubeId = p.loadURDF("cube_small.urdf", [0.1, 0.05, 1.6], [1, 0, 0, 0],
True, True)
#use official anchor feature for deformable body
# p.createSoftBodyAnchor(clothId ,3,cubeId,-1, [0.5,-0.5,0])
# p.setRealTimeSimulation(1)
while p.isConnected():
keys = p.getKeyboardEvents()
if ord('q') in keys and keys[ord('q')] & p.KEY_WAS_TRIGGERED:
break
if ord(' ') in keys and keys[ord(' ')] & p.KEY_WAS_TRIGGERED:
runSimulation = not runSimulation
if runSimulation:
p.stepSimulation()
# p.setGravity(0,0,-10)
# sleep(1./240.)
return
def getAction():
keys = p.getKeyboardEvents()
if p.B3G_UP_ARROW in keys and keys[p.B3G_UP_ARROW] & p.KEY_IS_DOWN:
return 0
elif p.B3G_LEFT_ARROW in keys and keys[p.B3G_LEFT_ARROW] & p.KEY_IS_DOWN:
return 1
elif p.B3G_RIGHT_ARROW in keys and keys[p.B3G_RIGHT_ARROW] & p.KEY_IS_DOWN:
return 2
else:
return 0
def get_key_pressed(self, relevant=None):
pressed_keys = []
#if configs.DISPLAY_UI:
# events = pygame.event.get()
# pressed_keys = [e.key for e in events if e.type == pygame.KEYDOWN]
events = p.getKeyboardEvents()
key_codes = events.keys()
for key in key_codes:
pressed_keys.append(key)
return pressed_keys
def checkkeyboardinput(): # Put keyboard control here
global quitApp, anchorPos, guiDisp, guiClock, jointCurPos
keys = p.getKeyboardEvents()
if p.B3G_BACKSPACE in keys:
quitApp = True
if ord('m') in keys and time.clock() - guiClock > guiMinTime:
p.configureDebugVisualizer(p.COV_ENABLE_GUI, guiDisp)
guiDisp = not guiDisp
guiClock = time.clock()
if p.B3G_LEFT_ARROW in keys:
anchorPos = list(
map(
sum,
zip(anchorPos, [
-x * speed
for x in np.cross(list(p.getDebugVisualizerCamera()[5]),
list(p.getDebugVisualizerCamera()[4]))
])))
p.changeConstraint(anchor, anchorPos)
if p.B3G_RIGHT_ARROW in keys:
anchorPos = list(
map(
sum,
zip(anchorPos, [
x * speed
for x in np.cross(list(p.getDebugVisualizerCamera()[5]),
list(p.getDebugVisualizerCamera()[4]))
])))
p.changeConstraint(anchor, anchorPos)
if p.B3G_DOWN_ARROW in keys:
anchorPos = list(
map(
sum,
zip(anchorPos, [
-x * speed for x in list(p.getDebugVisualizerCamera()[5])
])))
p.changeConstraint(anchor, anchorPos)
if p.B3G_UP_ARROW in keys:
anchorPos = list(
map(
sum,
zip(anchorPos,
[x * speed
for x in list(p.getDebugVisualizerCamera()[5])])))
p.changeConstraint(anchor, anchorPos)
if ord('p') in keys:
jointCurPos = [0] * 19
for i, ji in enumerate(jointIndices):
if ord(jointKeys[i]) in keys and p.B3G_SHIFT not in keys:
jointCurPos[i] += jointSpeed * pi
elif ord(jointKeys[i]) in keys and p.B3G_SHIFT in keys:
jointCurPos[i] -= jointSpeed * pi
p.setJointMotorControl2(hand, ji, p.POSITION_CONTROL, jointCurPos[i])
def _step(self, action):
p.stepSimulation()
if self._renders:
time.sleep(self.timeStep)
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
turn = -0.5
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
turn = 0
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
turn = 0.5
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
turn = 0
if (k == p.B3G_UP_ARROW and (v & p.KEY_WAS_TRIGGERED)):
self.forward = 0.05
if (k == p.B3G_UP_ARROW and (v & p.KEY_WAS_RELEASED)):
self.forward = 0.0
if (k == p.B3G_DOWN_ARROW and (v & p.KEY_WAS_TRIGGERED)):
self.forward = -0.05
if (k == p.B3G_DOWN_ARROW and (v & p.KEY_WAS_RELEASED)):
self.forward = 0.0
self.offset_command = self.offset_command + self.forward
# print(self.offset_command)
#print(keys)
theta, theta_dot, x, x_dot = p.getJointState(
self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
self.acceleration = action[0] - self.last_velocity
self.last_velocity = action[0]
self.jerk = self.acceleration - self.last_acceleration
self.last_acceleration = self.acceleration
x = x + self.offset_command
self.state = (theta, theta_dot, x, x_dot)
p.setJointMotorControl2(self.cartpole,
0,
p.VELOCITY_CONTROL,
targetVelocity=action,
velocityGain=1)
done = x < -self.x_threshold \
or x > self.x_threshold \
or theta < -self.theta_threshold_radians \
or theta > self.theta_threshold_radians
reward = 1 - math.fabs(x) / 2.4 - math.fabs(self.jerk) / 1.5
if self._renders:
#print(math.fabs(x)/2.4)
#print(math.fabs(self.acceleration)*1)
print(self.jerk)
return np.array(self.state), reward, done, {}
def simulate(sp):
# Global body params
visual_shape_id = -1
use_maximal_coordinates = 0
sim_time = 5
# Create target (i.e. pollen) shape and body
target = p.createCollisionShape(p.GEOM_SPHERE,
radius=sp['t_rad'],
physicsClientId=sp['pci'])
tuid = p.createMultiBody(sp['t_m'],
target,
visual_shape_id,
sp['t_pos'],
useMaximalCoordinates=use_maximal_coordinates,
physicsClientId=sp['pci'])
p.changeDynamics(tuid, -1, restitution=1 - 0.0001)
# Create projectile (i.e. air) shape and body
proj_rad = 0.15
proj_mass = 0.01
proj_pos = [0, 0, 0.9]
proj = p.createCollisionShape(p.GEOM_SPHERE,
radius=sp['p_rad'],
physicsClientId=sp['pci'])
puid = p.createMultiBody(sp['p_m'],
proj,
visual_shape_id,
sp['p_pos'],
useMaximalCoordinates=use_maximal_coordinates,
physicsClientId=sp['pci'])
p.changeDynamics(puid, -1, restitution=1 - 0.0001)
# Give projectile initial velocity
p.resetBaseVelocity(puid, sp['lv'], sp['av'], physicsClientId=sp['pci'])
# Start simulation
p.setGravity(0, 0, 0, physicsClientId=sp['pci']
) # ignore gravity since this is orientation-agnostic
first = True
while first or time.time() - start < sp['sim_time']:
p.stepSimulation(physicsClientId=sp['pci'])
foo = len(p.getContactPoints(physicsClientId=sp['pci']))
if foo > 0 and first:
start = time.time()
first = False
keys = p.getKeyboardEvents()
time.sleep(0.001)
print 'bv' + str(sp['pci']) + ': ' + str(
p.getBaseVelocity(tuid, physicsClientId=sp['pci']))
return p.getBaseVelocity(tuid, physicsClientId=sp['pci'])
def debug(self, pause=True):
keys = self.pbc.getKeyboardEvents()
if pause:
space_key = ord(' ')
if space_key in keys and keys[
space_key] & self.pbc.KEY_WAS_TRIGGERED:
print("Simulation Paused!")
print("Press Space key to start again!")
while True:
keys = pb.getKeyboardEvents()
if space_key in keys and keys[
space_key] & self.pbc.KEY_WAS_TRIGGERED:
break
def run(self):
#listener = keyboard.Listener(on_press=self.on_press,
# on_release=self.on_release)
#listener.start()
handle = self.pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP,
resolution=Camera.K_720p)
while True:
events = p.getKeyboardEvents()
self.on_press(events)
self.on_release(events)
img = self.pepper.getCameraFrame(handle)
cv2.imshow('Human Vision', img)
cv2.waitKey(1)
def run_continouous_simulation(self):
run_sim = True
# q_key = ord('q')
enter_key = p.B3G_RETURN
while (run_sim):
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (k == enter_key and (v & p.KEY_WAS_TRIGGERED)):
run_sim = False
print("Pressed Enter. Exit")
p.stepSimulation()
if self.visualization_realtime:
time.sleep(self.physics_ts)
def main():
args = parse_args()
env_name, env = setup_env(args.config_path)
env = gym.make(env_name)
env.render()
observation = env.reset()
start_pos, orient = env.robot.get_pos_orient(env.robot.right_end_effector)
start_rpy = env.get_euler(orient)
target_pos_offset = np.zeros(3)
target_rpy_offset = np.zeros(3)
while True:
keys = p.getKeyboardEvents(env.id)
# Process position movement keys ('u', 'i', 'o', 'j', 'k', 'l')
for key, action in POS_KEYS_ACTIONS.items():
if p.B3G_SHIFT not in keys and key in keys and keys[key] & p.KEY_IS_DOWN:
target_pos_offset += action
# Process rpy movement keys (shift + movement keys)
for key, action in RPY_KEYS_ACTIONS.items():
if (
p.B3G_SHIFT in keys
and keys[p.B3G_SHIFT] & p.KEY_IS_DOWN
and (key in keys and keys[key] & p.KEY_IS_DOWN)
):
target_rpy_offset += action
# print('Target position offset:', target_pos_offset, 'Target rpy offset:', target_rpy_offset)
target_pos = start_pos + target_pos_offset
target_rpy = start_rpy + target_rpy_offset
# Use inverse kinematics to compute the joint angles for the robot's arm
# so that its end effector moves to the target position.
target_joint_angles = env.robot.ik(
env.robot.right_end_effector,
target_pos,
env.get_quaternion(target_rpy),
env.robot.right_arm_ik_indices,
max_iterations=200,
use_current_as_rest=True,
)
# Get current joint angles of the robot's arm
current_joint_angles = env.robot.get_joint_angles(
env.robot.right_arm_joint_indices
)
# Compute the action as the difference between target and current joint angles.
action = (target_joint_angles - current_joint_angles) * 10
# Step the simulation forward
observation, reward, done, info = env.step(action)
def moveUR5Keyboard(robotID, wings, gotoWing):
"""
Change UR5 arm position based on keyboard input
"""
keys = p.getKeyboardEvents()
if ord(b'h') in keys:
gotoWing(robotID, wings["home"])
return
if ord(b'u') in keys:
gotoWing(robotID, wings["up"])
return
if ord(b'n') in keys:
gotoWing(robotID, wings["down"])
return
def get_keyboard_events(self):
events = p.getKeyboardEvents(physicsClientId=self.sim.id)
keys = {}
for keycode, valcode in events.items():
if valcode == 3:
val = 1
elif valcode == 1:
val = 0
elif valcode == 4:
val = -1
else:
continue
key = Key(keycode)
keys[key] = val
return keys
def set_speed_from_keyboard(self):
keys = p.getKeyboardEvents()
self.velocity_x = 0
self.velocity_rotate = 0
if p.B3G_UP_ARROW in keys:
self.velocity_x = self.velocity_x + 1
if p.B3G_LEFT_ARROW in keys:
self.velocity_rotate = self.velocity_rotate - 1
if p.B3G_DOWN_ARROW in keys:
self.velocity_x = self.velocity_x - 1
if p.B3G_RIGHT_ARROW in keys:
self.velocity_rotate = self.velocity_rotate + 1
self.velocity_x = self.velocity_x * self.motor_const_velocity
self.velocity_rotate = self.velocity_rotate * self.motor_const_velocity
self.set_motor()
def _key_events_fetcher(self):
"""Fetch and decode keyboard events in Bullet GUI.
Returns:
events: list of (key, key_act, modifiers) tuples.
"""
# Fetch events
bullet_events = p.getKeyboardEvents()
# Map event id to key name
if self._key_dict is None:
key_dict = {}
for i in xrange(128):
key_dict[i] = str(unichr(i))
key_dict[65295] = 'LEFT'
key_dict[65296] = 'RIGHT'
key_dict[65297] = 'UP'
key_dict[65298] = 'DOWN'
key_dict[65309] = 'ENTER'
self._key_dict = key_dict
# Map event id to key activity name
if self._key_act_dict is None:
key_act_dict = {}
key_act_dict[1] = 'DOWN'
key_act_dict[2] = 'TRIGGERED'
key_act_dict[3] = 'UNKNOWN(3L)'
key_act_dict[4] = 'RELEASED'
key_act_dict[5] = 'UNKNOWN(5L)'
key_act_dict[6] = 'UNKNOWN(6L)'
self._key_act_dict = key_act_dict
# Map event id to modifier key names
if self._modifier_dict is None:
modifier_dict = {}
modifier_dict[65306] = 'SHIFT'
modifier_dict[65307] = 'CTRL'
modifier_dict[65308] = 'OPTION'
self._modifier_dict = modifier_dict
# Decode events
events = []
for key, key_act in bullet_events.items():
if self._key_dict.has_key(key):
key_name = self._key_dict[key]
elif self._modifier_dict.has_key(key):
key_name = self._modifier_dict[key]
else:
key_name = None
key_act_name = self._key_act_dict[key_act]
events.append((key_name, key_act_name))
return events
def restoreOnKeyboard(world_states, x1, y1, o1):
"""
Restore to last saved state when 'r' is pressed
"""
keys = p.getKeyboardEvents()
if ord(b'r') in keys:
print("Pressed R")
if len(world_states) != 0:
print("Restoring state")
world_states.pop()
id1, x, y, o = world_states[-1]
p.restoreState(stateId=id1)
# q=p.getQuaternionFromEuler((0,0,0))
# p.resetBasePositionAndOrientation(([0, 0, 0], q)) # Get robot to home when undo
return x, y, o, world_states
return x1, y1, o1, world_states
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
print(p.getNumJoints(boxId))
for joint in range(p.getNumJoints(boxId)):
targetVelocity = 10
if (i % 2):
targetVelocity = -10
p.setJointMotorControl2(boxId,
joint,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=100)
segmentStart = segmentStart - 1.1
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1):
camData = p.getDebugVisualizerCamera()
viewMat = camData[2]
projMat = camData[3]
p.getCameraImage(256,
256,
viewMatrix=viewMat,
projectionMatrix=projMat,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
keys = p.getKeyboardEvents()
p.stepSimulation()
#print(keys)
time.sleep(0.01)