Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def reset(self):
"""
Reset the environment
"""
objects = p.loadSDF(os.path.join(self.urdf_root_path, "kuka_iiwa/kuka_with_gripper2.sdf"))
self.kuka_uid = objects[0]
p.resetBasePositionAndOrientation(self.kuka_uid, [-0.100000, 0.000000, -0.15],
[0.000000, 0.000000, 0.000000, 1.000000])
self.joint_positions = [0.006418, 0.113184, -0.011401, -1.289317, 0.005379, 1.737684, -0.006539, 0.000048,
-0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200]
self.num_joints = p.getNumJoints(self.kuka_uid)
for jointIndex in range(self.num_joints):
p.resetJointState(self.kuka_uid, jointIndex, self.joint_positions[jointIndex])
p.setJointMotorControl2(self.kuka_uid, jointIndex, p.POSITION_CONTROL,
targetPosition=self.joint_positions[jointIndex], force=self.max_force)
self.end_effector_pos = np.array([0.537, 0.0, 0.5])
self.end_effector_angle = 0
self.motor_names = []
self.motor_indices = []
for i in range(self.num_joints):
joint_info = p.getJointInfo(self.kuka_uid, i)
q_index = joint_info[3]
if q_index > -1:
self.motor_names.append(str(joint_info[1]))
self.motor_indices.append(i)
def gripper(self, cmd, mode=pb.POSITION_CONTROL):
'''
Gripper commands need to be mirrored to simulate behavior of the actual
UR5. Converts one command input to 6 joint positions, used for the
robotiq gripper. This is a rough simulation of the way the robotiq
gripper works in practice, in the absence of a plugin like the one we
use in Gazebo.
Parameters:
-----------
cmd: 1x1 array of floating point position commands in [-0.8, 0]
mode: PyBullet control mode
'''
cmd = cmd[0]
# This is actually only a 1-DOF gripper
if cmd < -0.1:
def arm(self, cmd, mode=pb.POSITION_CONTROL):
'''
Set joint commands for the robot arm.
'''
if len(cmd) > 6:
raise RuntimeError('too many joint positions')
for i, q in enumerate(cmd):
pb.setJointMotorControl2(self.handle, i, mode, q)
def _setupRobot(self, handle):
'''
Configure the robot so that it is ready to begin the task. Robot should
be oriented so the gripper is near the cluttered area with the mug.
'''
self.robot.place([0, 0, 0], [0, 0, 0, 1], self.joint_positions)
self.robot2 = self.cloneRobot()
self.robot2.load()
self.robot2.place([-1, 0, 0], [0, 0, 1, 0],
self.joint_positions)
self.robot.arm(self.joint_positions, pb.POSITION_CONTROL)
self.robot.gripper(0, pb.POSITION_CONTROL)
def set_gripper_open_position(self, indices, positions, set_instantly=False, force=500):
if set_instantly:
self.set_joint_angles(indices, positions, use_limits=True)
p.setJointMotorControlArray(self.body, jointIndices=indices, controlMode=p.POSITION_CONTROL, targetPositions=positions, positionGains=np.array([0.05]*len(indices)), forces=[force]*len(indices), physicsClientId=self.id)
p.setPhysicsEngineParameter(solverResidualThreshold=1e-2)
index = 0
numX = 10
numY = 10
for i in range (numX):
for j in range (numY):
print("loading animal ", index)
index+=1
#anymal = p.loadURDF("atlas/atlas_v4_with_multisense.urdf",[i*3,j*3,1], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
anymal = p.loadURDF("ANYmal/robot.urdf",[(i-numX/2)*2,(j-numY/2)*2,0.6], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES, useMaximalCoordinates=False)
for j in range(p.getNumJoints(anymal)):
p.setJointMotorControl2(anymal,j,p.POSITION_CONTROL,targetPosition=0, force=500)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.setGravity(0,0,-10)
#p.setRealTimeSimulation(1)
while (1):
p.stepSimulation()
time.sleep(1./240.)
def _setupRobot(self, handle):
self.robot.place([0, 0, 0], [0, 0, 0, 1], self.joint_positions)
self.robot.arm(self.joint_positions, pb.POSITION_CONTROL)
self.robot.gripper(0, pb.POSITION_CONTROL)
def control_joints(body, joints, positions):
# TODO: the whole PR2 seems to jitter
#kp = 1.0
#kv = 0.3
#forces = [get_max_force(body, joint) for joint in joints]
#forces = [5000]*len(joints)
#forces = [20000]*len(joints)
return p.setJointMotorControlArray(body, joints, p.POSITION_CONTROL,
targetPositions=positions,
targetVelocities=[0.0] * len(joints),
physicsClientId=CLIENT) #,
#positionGains=[kp] * len(joints),
def _setupRobot(self, handle):
self.robot.place([0, 0, 0], [0, 0, 0, 1], self.joint_positions)
self.robot.arm(self.joint_positions, pb.POSITION_CONTROL)
self.robot.gripper(0, pb.POSITION_CONTROL)
def set_position(self, position):
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.POSITION_CONTROL, targetPosition=position)