Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
import pybullet as p
import time,math
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
atlas = p.loadURDF("atlas/atlas_v4_with_multisense.urdf", [-2,3,-0.5])
for i in range (p.getNumJoints(atlas)):
p.setJointMotorControl2(atlas,i,p.POSITION_CONTROL,0)
print(p.getJointInfo(atlas,i))
if 1:
objs = p.loadSDF("botlab/botlab.sdf", globalScaling=2.0)
zero=[0,0,0]
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
print("converting y to z axis")
for o in objs:
pos,orn = p.getBasePositionAndOrientation(o)
y2x = p.getQuaternionFromEuler([3.14/2.,0,3.14/2])
newpos,neworn = p.multiplyTransforms(zero,y2x,pos,orn)
p.resetBasePositionAndOrientation(o,newpos,neworn)
else:
p.loadURDF("plane.urdf",[0,0,-3])
p.loadURDF("boston_box.urdf",[-2,3,-2], useFixedBase=True)
if self.ordered_joints is not None:
ordered_joints = self.ordered_joints
else:
ordered_joints = []
part_name, robot_name = p.getBodyInfo(bodies[0])
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name, bodies, 0, -1, self.scale, model_type=self.model_type)
# By default, use base_link as self.robot_body
if self.robot_name == part_name:
self.robot_body = parts[part_name]
for j in range(p.getNumJoints(bodies[0])):
p.setJointMotorControl2(bodies[0], j, p.POSITION_CONTROL, positionGain=0.1, velocityGain=0.1, force=0)
_, joint_name, joint_type, _, _, _, _, _, _, _, _, _, part_name, _, _, _, _ = p.getJointInfo(bodies[0], j)
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name, bodies, 0, j, self.scale, model_type=self.model_type)
# If self.robot_name is not base_link, but a body part, use it as self.robot_body
if self.robot_name == part_name:
self.robot_body = parts[part_name]
if joint_name[:6] == "ignore":
Joint(joint_name, bodies, 0, j, self.scale, model_type=self.model_type).disable_motor()
continue
if joint_name[:8] != "jointfix" and joint_type != p.JOINT_FIXED:
joints[joint_name] = Joint(joint_name, bodies, 0, j, self.scale, model_type=self.model_type)
ordered_joints.append(joints[joint_name])
self.scale,
model_type=self.model_type)
# By default, use base_link as self.robot_body
if self.robot_name == part_name:
self.robot_body = parts[part_name]
for j in range(p.getNumJoints(bodies[0])):
robot_mass += p.getDynamicsInfo(bodies[0], j)[0]
p.setJointMotorControl2(bodies[0],
j,
p.POSITION_CONTROL,
positionGain=0.1,
velocityGain=0.1,
force=0)
_, joint_name, joint_type, _, _, _, _, _, _, _, _, _, part_name, _, _, _, _ = p.getJointInfo(
bodies[0], j)
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name,
bodies,
0,
j,
self.scale,
model_type=self.model_type)
# If self.robot_name is not base_link, but a body part, use it as self.robot_body
if self.robot_name == part_name:
self.robot_body = parts[part_name]
if joint_name[:6] == "ignore":
def _BuildJointNameToIdDict(self):
num_joints = p.getNumJoints(self.minitaur)
self.joint_name_to_id = {}
for i in range(num_joints):
joint_info = p.getJointInfo(self.minitaur, i)
self.joint_name_to_id[joint_info[1].decode("UTF-8")] = joint_info[0]
def _BuildJointNameToIdDict(self):
num_joints = p.getNumJoints(self.minitaur)
self._joint_name_to_id = {}
for i in range(num_joints):
joint_info = p.getJointInfo(self.minitaur, i)
self._joint_name_to_id[joint_info[1].decode("UTF-8")] = joint_info[0]
self.controllable_joint_lower_limits[:len(self.wheel_joint_indices)] = -np.inf
self.controllable_joint_upper_limits[:len(self.wheel_joint_indices)] = np.inf
self.right_arm_lower_limits = [self.lower_limits[i] for i in self.right_arm_joint_indices]
self.right_arm_upper_limits = [self.upper_limits[i] for i in self.right_arm_joint_indices]
self.left_arm_lower_limits = [self.lower_limits[i] for i in self.left_arm_joint_indices]
self.left_arm_upper_limits = [self.upper_limits[i] for i in self.left_arm_joint_indices]
self.joint_max_forces = self.get_joint_max_force(self.controllable_joint_indices)
# Determine ik indices for the right and left arms (indices differ since fixed joints are not counted)
self.right_arm_ik_indices = []
self.left_arm_ik_indices = []
for i in self.right_arm_joint_indices:
counter = 0
for j in self.all_joint_indices:
if i == j:
self.right_arm_ik_indices.append(counter)
joint_type = p.getJointInfo(self.body, j, physicsClientId=self.id)[2]
if joint_type != p.JOINT_FIXED:
counter += 1
for i in self.left_arm_joint_indices:
counter = 0
for j in self.all_joint_indices:
if i == j:
self.left_arm_ik_indices.append(counter)
joint_type = p.getJointInfo(self.body, j, physicsClientId=self.id)[2]
if joint_type != p.JOINT_FIXED:
counter += 1
def enforce_hard_human_joint_limits(self):
if not self.human_controllable_joint_indices:
return
# Enforce joint limits. Sometimes, external forces and break the person's hard joint limits.
joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id)
joint_positions = np.array([x[0] for x in joint_states])
if self.human_joint_lower_limits is None:
self.human_joint_lower_limits = []
self.human_joint_upper_limits = []
for i, j in enumerate(self.human_controllable_joint_indices):
joint_info = p.getJointInfo(self.human, j, physicsClientId=self.id)
joint_name = joint_info[1]
joint_pos = joint_positions[i]
lower_limit = joint_info[8]
upper_limit = joint_info[9]
self.human_joint_lower_limits.append(lower_limit)
self.human_joint_upper_limits.append(upper_limit)
# print(joint_name, joint_pos, lower_limit, upper_limit)
for i, j in enumerate(self.human_controllable_joint_indices):
if joint_positions[i] < self.human_joint_lower_limits[i]:
p.resetJointState(self.human, jointIndex=j, targetValue=self.human_joint_lower_limits[i], targetVelocity=0, physicsClientId=self.id)
elif joint_positions[i] > self.human_joint_upper_limits[i]:
p.resetJointState(self.human, jointIndex=j, targetValue=self.human_joint_upper_limits[i], targetVelocity=0, physicsClientId=self.id)