Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def set_state(self, x, vx):
"""Set state of joint
x is defined in real world scale """
if self.joint_type == p.JOINT_PRISMATIC:
x /= self.scale
vx /= self.scale
p.resetJointState(self.bodies[self.body_index], self.joint_index, x, vx)
def enforce_joint_limits(self, indices=None):
if indices is None:
indices = self.all_joint_indices
joint_angles = self.get_joint_angles_dict(indices)
if self.lower_limits is None or len(indices) > len(self.lower_limits):
self.update_joint_limits()
for j in indices:
if joint_angles[j] < self.lower_limits[j]:
p.resetJointState(self.body, jointIndex=j, targetValue=self.lower_limits[j], targetVelocity=0, physicsClientId=self.id)
elif joint_angles[j] > self.upper_limits[j]:
p.resetJointState(self.body, jointIndex=j, targetValue=self.upper_limits[j], targetVelocity=0, physicsClientId=self.id)
def set_state(self, x, vx):
p.resetJointState(self.bodies[self.bodyIndex], self.jointIndex, x, vx)
if 3 in self.human_controllable_joint_indices:
# Right human arm
tz, tx, ty, qe = [j[0] for j in p.getJointStates(self.human, jointIndices=[3, 4, 5, 6], physicsClientId=self.id)]
# Transform joint angles to match those from the Matlab data
tz2 = (-tz + 2*np.pi) % (2*np.pi)
tx2 = (tx + 2*np.pi) % (2*np.pi)
ty2 = -ty
qe2 = (-qe + 2*np.pi) % (2*np.pi)
result = self.human_limits_model.predict_classes(np.array([[tz2, tx2, ty2, qe2]]))
if result == 1:
# This is a valid pose for the person
self.right_arm_previous_valid_pose = [tz, tx, ty, qe]
elif result == 0 and self.right_arm_previous_valid_pose is not None:
# The person is in an invalid pose. Move them back to the most recent valid pose.
for i, j in enumerate([3, 4, 5, 6]):
p.resetJointState(self.human, jointIndex=j, targetValue=self.right_arm_previous_valid_pose[i], targetVelocity=0, physicsClientId=self.id)
if 13 in self.human_controllable_joint_indices:
# Left human arm
tz, tx, ty, qe = [j[0] for j in p.getJointStates(self.human, jointIndices=[13, 14, 15, 16], physicsClientId=self.id)]
# Transform joint angles to match those from the Matlab data
tz2 = (tz + 2*np.pi) % (2*np.pi)
tx2 = (tx + 2*np.pi) % (2*np.pi)
ty2 = ty
qe2 = (-qe + 2*np.pi) % (2*np.pi)
result = self.human_limits_model.predict_classes(np.array([[tz2, tx2, ty2, qe2]]))
if result == 1:
# This is a valid pose for the person
self.left_arm_previous_valid_pose = [tz, tx, ty, qe]
elif result == 0 and self.left_arm_previous_valid_pose is not None:
# The person is in an invalid pose. Move them back to the most recent valid pose.
for i, j in enumerate([13, 14, 15, 16]):
p.resetJointState(self.human, jointIndex=j, targetValue=self.left_arm_previous_valid_pose[i], targetVelocity=0, physicsClientId=self.id)
def set_state(self, x, vx):
"""Set state of joint
x is defined in real world scale """
if self.joint_type == p.JOINT_PRISMATIC:
x /= self.scale
vx /= self.scale
p.resetJointState(self.bodies[self.body_index], self.joint_index, x, vx)
#right back leg
p.resetJointState(quadruped,12,1.57)
p.resetJointState(quadruped,14,-2.2)
p.resetJointState(quadruped,15,-1.57)
p.resetJointState(quadruped,17,2.2)
p.createConstraint(quadruped,14,quadruped,17,p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,1.57,1)
p.setJointMotorControl(quadruped,13,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,14,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,16,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,17,p.VELOCITY_CONTROL,0,0)
#left back leg
p.resetJointState(quadruped,18,1.57)
p.resetJointState(quadruped,20,-2.2)
p.resetJointState(quadruped,21,-1.57)
p.resetJointState(quadruped,23,2.2)
p.createConstraint(quadruped,20,quadruped,23,p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,1.57,1)
p.setJointMotorControl(quadruped,19,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,20,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,22,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0)
p_gain = 2
speed = 10
def _ResetPoseForLeg(self, leg_id, add_constraint):
"""Reset the initial pose for the leg.
Args:
leg_id: It should be 0, 1, 2, or 3, which represents the leg at
front_left, back_left, front_right and back_right.
add_constraint: Whether to add a constraint at the joints of two feet.
"""
knee_friction_force = 0
half_pi = math.pi / 2.0
knee_angle = -2.1834
leg_position = self.LEG_POSITION[leg_id]
p.resetJointState(self.minitaur,
self.joint_name_to_id["motor_" + leg_position + "L_joint"],
self.motor_direction[2 * leg_id] * half_pi,
targetVelocity=0)
p.resetJointState(self.minitaur,
self.joint_name_to_id["knee_" + leg_position + "L_link"],
self.motor_direction[2 * leg_id] * knee_angle,
targetVelocity=0)
p.resetJointState(self.minitaur,
self.joint_name_to_id["motor_" + leg_position + "R_joint"],
self.motor_direction[2 * leg_id + 1] * half_pi,
targetVelocity=0)
p.resetJointState(self.minitaur,
self.joint_name_to_id["knee_" + leg_position + "R_link"],
self.motor_direction[2 * leg_id + 1] * knee_angle,
targetVelocity=0)
if add_constraint:
knee_friction_force = 0
half_pi = math.pi / 2.0
knee_angle = -2.1834
leg_position = LEG_POSITION[leg_id]
pybullet.resetJointState(
self.quadruped,
self._joint_name_to_id["motor_" + leg_position + "L_joint"],
self._motor_direction[2 * leg_id] * half_pi,
targetVelocity=0)
pybullet.resetJointState(
self.quadruped,
self._joint_name_to_id["knee_" + leg_position + "L_link"],
self._motor_direction[2 * leg_id] * knee_angle,
targetVelocity=0)
pybullet.resetJointState(
self.quadruped,
self._joint_name_to_id["motor_" + leg_position + "R_joint"],
self._motor_direction[2 * leg_id + 1] * half_pi,
targetVelocity=0)
pybullet.resetJointState(
self.quadruped,
self._joint_name_to_id["knee_" + leg_position + "R_link"],
self._motor_direction[2 * leg_id + 1] * knee_angle,
targetVelocity=0)
if add_constraint:
pybullet.createConstraint(
self.quadruped, self._joint_name_to_id["knee_"
+ leg_position + "R_link"],
self.quadruped, self._joint_name_to_id["knee_"
+ leg_position + "L_link"],
pybullet.JOINT_POINT2POINT, [0, 0, 0],
def _force_joint_positions(self, joint_positions):
for i in range(5):
p.resetJointState(
self.arm,
i,
joint_positions[i]
)
for i in range(7, 9):
p.resetJointState(
self.arm,
i,
joint_positions[-1]
)
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,)
self.gripper(0)