Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
objs_name_to_add = np.random.choice(objs, randn)
objs_to_add = [os.path.join(sdf_dir, obj, self.model_file_name)
for obj in objs_name_to_add]
identity_orientation = pb.getQuaternionFromEuler([0,0,0])
# load sdfs for all objects and initialize positions
for obj_index, obj in enumerate(objs_to_add):
if objs_name_to_add[obj_index] in self.models:
try:
print 'Loading object: ', obj
obj_id_list = pb.loadSDF(obj)
for obj_id in obj_id_list:
self.objs.append(obj_id)
random_position = np.random.rand(3)*self.spawn_pos_delta + self.spawn_pos_min
pb.resetBasePositionAndOrientation(obj_id, random_position, identity_orientation)
except Exception, e:
print e
def _addTower(self, pos, blocks, urdf_dir):
'''
Helper function that generats a tower containing listed blocks at the
specific position
'''
z = 0.025
ids = []
for block in blocks:
urdf_filename = os.path.join(
urdf_dir, self.model, self.block_urdf % block)
obj_id = pb.loadURDF(urdf_filename)
pb.resetBasePositionAndOrientation(
obj_id,
(pos[0], pos[1], z),
(0, 0, 0, 1))
self.addObject("block", "%s_block" % block, obj_id)
z += 0.05
ids.append(obj_id)
return ids
base_pos=None,
endeffector_pos=None):
"""Resets the kuka base and joint positions.
Args:
base_pos: The [x, y, z] position of Kuka base.
endeffector_pos: The [x, y, z] position of the initial endeffector
position.
"""
# Default values for the base position and initial endeffector position.
if base_pos is None:
base_pos = [-0.1, 0.0, 0.07]
if endeffector_pos is None:
endeffector_pos = [0.537, 0.0, 0.5]
pybullet.resetBasePositionAndOrientation(self.kukaUid,
base_pos,
[0.000000, 0.000000, 0.000000, 1.000000],
physicsClientId=self.cid)
self.jointPositions=[0.006418, 0.413184, -0.011401, -1.589317, 0.005379,
1.137684, -0.006539, 0.000048, -0.299912, 0.000000,
-0.000043, 0.299960, 0.000000, -0.000200 ]
self.numJoints = pybullet.getNumJoints(self.kukaUid,physicsClientId=self.cid)
for jointIndex in range (self.numJoints):
pybullet.resetJointState(self.kukaUid,
jointIndex,
self.jointPositions[jointIndex],
physicsClientId=self.cid)
if self.useSimulation:
pybullet.setJointMotorControl2(self.kukaUid,
jointIndex,
pybullet.POSITION_CONTROL,
def _set_fields_of_pose_of(self, pos, orn):
"""Calls native pybullet method for setting real (scaled) robot body pose"""
p.resetBasePositionAndOrientation(self.bodies[self.body_index], np.array(pos) / self.scale, orn)
def _add_balls(self, num, filename, typename):
'''
Helper function to spawn a whole bunch of random balls.
'''
for i in xrange(num):
obj_id = pb.loadURDF(filename)
random_position = np.random.rand(
3) * self.spawn_pos_delta + self.spawn_pos_min
pb.resetBasePositionAndOrientation(
obj_id, random_position, (0, 0, 0, 1))
objname = "%s%03d" % (typename, i)
self.addObject(typename, obj_id)
def reset(self):
self.setup_timing()
self.task_success = 0
self.prev_target_contact_pos = np.zeros(3)
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_left_arm_joint_indices]
self.robot_upper_limits = self.robot_upper_limits[self.robot_left_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)
joints_positions = [(3, np.deg2rad(30)), (6, np.deg2rad(-90)), (16, np.deg2rad(-90)), (28, np.deg2rad(-90)), (31, np.deg2rad(80)), (35, np.deg2rad(-90)), (38, np.deg2rad(80))]
self.human_controllable_joint_indices = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
self.world_creation.setup_human_joints(self.human, joints_positions, self.human_controllable_joint_indices, use_static_joints=True, human_reactive_force=None if self.human_control else 1, human_reactive_gain=0.01)
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]
shoulder_pos, shoulder_orient = p.getLinkState(self.human, 5, computeForwardKinematics=True, physicsClientId=self.id)[:2]
elbow_pos, elbow_orient = p.getLinkState(self.human, 7, computeForwardKinematics=True, physicsClientId=self.id)[:2]
wrist_pos, wrist_orient = p.getLinkState(self.human, 9, computeForwardKinematics=True, physicsClientId=self.id)[:2]
if self.robot_type == 'pr2':
target_pos = np.array([-0.55, 0, 0.8]) + self.np_random.uniform(-0.05, 0.05, size=3)
def place(self, pos, rot, joints):
pb.resetBasePositionAndOrientation(self.handle, pos, rot)
pb.createConstraint(
self.handle, -1, -1, -1, pb.JOINT_FIXED, pos, [0, 0, 0], rot)
for i, q in enumerate(joints):
pb.resetJointState(self.handle, i, q)
# gripper
pb.resetJointState(self.handle, self.left_knuckle, 0)
pb.resetJointState(self.handle, self.right_knuckle, 0)
pb.resetJointState(self.handle, self.left_finger, 0)
pb.resetJointState(self.handle, self.right_finger, 0)
pb.resetJointState(self.handle, self.left_fingertip, 0)
pb.resetJointState(self.handle, self.right_fingertip, 0)
self.arm(joints,)
def reset_position(self, position):
p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, self.get_orientation())
Returns
-------
baxterId : int
endEffectorId : int
"""
p.resetSimulation()
# Load plane
p.loadURDF("plane.urdf", [0, 0, -1], useFixedBase=True)
sleep(0.1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
# Load Baxter
baxterId = p.loadURDF("baxter_common/baxter_description/urdf/toms_baxter.urdf", useFixedBase=True)
p.resetBasePositionAndOrientation(baxterId, [0.5, -0.8, 0.0], [0., 0., -1., -1.])
#p.resetBasePositionAndOrientation(baxterId, [0.5, -0.8, 0.0],[0,0,0,1])
#p.resetBasePositionAndOrientation(baxterId, [0, 0, 0], )
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
# Grab relevant joint IDs
endEffectorId = 48 # (left gripper left finger)
# Set gravity
p.setGravity(0., 0., -10.)
# Let the world run for a bit
for _ in range(initialSimSteps):
p.stepSimulation()
return baxterId, endEffectorId