Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
resetPose()
# Initialise the gamepad object using the gamepad inputs Python package
gamepad = ThreadedInputs()
for gamepadInput in gamepadInputs:
gamepad.append_command(gamepadInput, gamepadInputs[gamepadInput])
gamepad.start()
trotting=TrottingGait()
s=False
while True:
bodyPos=robot.getPos()
bodyOrn,_,_=robot.getIMU()
xr,yr,_= p.getEulerFromQuaternion(bodyOrn)
distance=math.sqrt(bodyPos[0]**2+bodyPos[1]**2)
if distance>50:
robot.resetBody()
ir=xr/(math.pi/180)
handleGamepad()
d=time.time()-rtime
height = p.readUserDebugParameter(IDheight)
# wait 3 seconds to start
if d>3:
robot.feetPosition(trotting.positions(d-3))
else:
robot.feetPosition(Lp)
#roll=-xr
roll=0
def rpy(self):
return pybullet.getEulerFromQuaternion(self.body_part.current_orientation())
def getObservation(self):
"""
Returns the position and angle of the effector
:return: ([float])
"""
observation = []
state = p.getLinkState(self.kuka_uid, self.kuka_gripper_index)
pos = state[0]
orn = state[1]
euler = p.getEulerFromQuaternion(orn)
observation.extend(list(pos))
observation.extend(list(euler))
return observation
def _compute_observation(self):
cubePos, cubeOrn = p.getBasePositionAndOrientation(self.botId)
cubeEuler = p.getEulerFromQuaternion(cubeOrn)
linear, angular = p.getBaseVelocity(self.botId)
return [cubeEuler[0],angular[0],self.vt]
if (v[2] == p.GEOM_SPHERE):
urdfCollision.geom_radius = v[3][0]
if (v[2] == p.GEOM_MESH):
urdfCollision.geom_meshfilename = v[4].decode("utf-8")
if urdfCollision.geom_meshfilename == UNKNOWN_FILE:
continue
urdfCollision.geom_meshscale = v[3]
if (v[2] == p.GEOM_CYLINDER):
urdfCollision.geom_length = v[3][0]
urdfCollision.geom_radius = v[3][1]
if (v[2] == p.GEOM_CAPSULE):
urdfCollision.geom_length = v[3][0]
urdfCollision.geom_radius = v[3][1]
pos, orn = p.multiplyTransforms(dyn[3], dyn[4], v[5], v[6])
urdfCollision.origin_xyz = pos
urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
urdfLink.urdf_collision_shapes.append(urdfCollision)
# step simulation forward. at the end of each repeat we set part of the step's
# state by capture the cart & pole state in some form.
for r in xrange(self.repeats):
for _ in xrange(self.steps_per_repeat):
p.stepSimulation()
p.applyExternalForce(self.cart, -1, (fx,fy,0), (0,0,0), p.WORLD_FRAME)
if self.delay > 0:
time.sleep(self.delay)
self.set_state_element_for_repeat(r)
self.steps += 1
# Check for out of bounds by position or orientation on pole.
# we (re)fetch pose explicitly rather than depending on fields in state.
(x, y, _z), orient = p.getBasePositionAndOrientation(self.pole)
(cx, cy, cz), corient = p.getBasePositionAndOrientation(self.cart)
ox, oy, _oz = p.getEulerFromQuaternion(orient) # roll / pitch / yaw
if abs(x) > self.pos_threshold or abs(y) > self.pos_threshold or abs(cx) > self.pos_threshold or abs(cy) > self.pos_threshold: # cart and pole can not escape a certain box
info['done_reason'] = 'out of position bounds'
self.done = True
reward = 0.0
elif abs(ox) > self.angle_threshold or abs(oy) > self.angle_threshold:
# TODO: probably better to do explicit angle from z?
info['done_reason'] = 'out of orientation bounds'
self.done = True
reward = 0.0
# check for end of episode (by length)
if self.steps >= self.max_episode_len:
info['done_reason'] = 'episode length'
self.done = True
# calc reward, fixed base of 1.0
reward = 1.0
def ik_random_restarts(self, body, target_joint, target_pos, target_orient, world_creation, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, ik_indices=range(29, 29+7), max_iterations=1000, max_ik_random_restarts=50, random_restart_threshold=0.01, half_range=False, step_sim=False, check_env_collisions=False):
orient_orig = target_orient
best_ik_joints = None
best_ik_distance = 0
for r in range(max_ik_random_restarts):
target_joint_positions = self.ik(body, target_joint, target_pos, target_orient, ik_indices=ik_indices, max_iterations=max_iterations, half_range=half_range)
world_creation.setup_robot_joints(body, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, randomize_joint_positions=False, default_positions=np.array(target_joint_positions), tool=None)
if step_sim:
for _ in range(5):
p.stepSimulation(physicsClientId=self.id)
if len(p.getContactPoints(bodyA=body, bodyB=body, physicsClientId=self.id)) > 0 and orient_orig is not None:
# The robot's arm is in contact with itself. Continually randomize end effector orientation until a solution is found
target_orient = p.getQuaternionFromEuler(p.getEulerFromQuaternion(orient_orig, physicsClientId=self.id) + np.deg2rad(self.np_random.uniform(-45, 45, size=3)), physicsClientId=self.id)
if check_env_collisions:
for _ in range(25):
p.stepSimulation(physicsClientId=self.id)
gripper_pos, gripper_orient = p.getLinkState(body, target_joint, computeForwardKinematics=True, physicsClientId=self.id)[:2]
if np.linalg.norm(target_pos - np.array(gripper_pos)) < random_restart_threshold and (target_orient is None or np.linalg.norm(target_orient - np.array(gripper_orient)) < random_restart_threshold or np.isclose(np.linalg.norm(target_orient - np.array(gripper_orient)), 2, atol=random_restart_threshold)):
return True, np.array(target_joint_positions)
if best_ik_joints is None or np.linalg.norm(target_pos - np.array(gripper_pos)) < best_ik_distance:
best_ik_joints = target_joint_positions
best_ik_distance = np.linalg.norm(target_pos - np.array(gripper_pos))
world_creation.setup_robot_joints(body, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, randomize_joint_positions=False, default_positions=np.array(best_ik_joints), tool=None)
return False, np.array(best_ik_joints)
# step simulation forward. at the end of each repeat we set part of the step's
# state by capture the cart & pole state in some form.
for r in xrange(self.repeats):
for _ in xrange(self.steps_per_repeat):
p.stepSimulation()
p.applyExternalForce(self.cart, -1, (fx,fy,0), (0,0,0), p.WORLD_FRAME)
if self.delay > 0:
time.sleep(self.delay)
self.set_state_element_for_repeat(r)
self.steps += 1
# Check for out of bounds by position or orientation on pole.
# we (re)fetch pose explicitly rather than depending on fields in state.
(x, y, _z), orient = p.getBasePositionAndOrientation(self.pole)
ox, oy, _oz = p.getEulerFromQuaternion(orient) # roll / pitch / yaw
if abs(x) > self.pos_threshold or abs(y) > self.pos_threshold:
info['done_reason'] = 'out of position bounds'
self.done = True
reward = 0.0
elif abs(ox) > self.angle_threshold or abs(oy) > self.angle_threshold:
# TODO: probably better to do explicit angle from z?
info['done_reason'] = 'out of orientation bounds'
self.done = True
reward = 0.0
# check for end of episode (by length)
if self.steps >= self.max_episode_len:
info['done_reason'] = 'episode length'
self.done = True
# calc reward, fixed base of 1.0
reward = 1.0
def getFrames(self):
"""Gets the available frames in the current robot model
Returns:
dict -- dict of str -> (pos, orientation)
"""
frames = {}
for name in self.frames.keys():
jointState = p.getLinkState(self.robot, self.frames[name])
pos = jointState[0]
orientation = p.getEulerFromQuaternion(jointState[1])
frames[name] = [pos, orientation]
return frames