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_kinematic_pose(u):
if isinstance(u, zencad.assemble.kinematic_unit):
info = p.getJointState(u.pybullet_base.boxId, u.simulation_hint2)
pose=info[0]
u.set_coord(pose + u.simulation_start_pose)
zencad.assemble.for_each_unit(self.base_interactive, set_kinematic_pose)
def GetMotorAngles(self):
"""Get the eight motor angles at the current moment.
Returns:
Motor angles.
"""
motor_angles = [
p.getJointState(self.minitaur, motor_id)[0]
for motor_id in self._motor_id_list
]
motor_angles = np.multiply(motor_angles, self._motor_direction)
return motor_angles
def getMotorVelocities(self):
motorVelocities = []
for i in range(self.nMotors):
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
motorVelocities.append(jointState[1])
motorVelocities = np.multiply(motorVelocities, self.motorDir)
return motorVelocities
def get_state(self):
"""Get state of joint
Position is defined in real world scale """
x, vx, _, trq = p.getJointState(self.bodies[self.body_index], self.joint_index)
if self.joint_type == p.JOINT_PRISMATIC:
x *= self.scale
vx *= self.scale
return x, vx, trq
buttonA = 0
t5 = mult*time.clock()
#if ((frameNr % 32) == 0):
# p.removeUserDebugItem(lines[i])
# for i in range (numLines):
# spacing = 0.01
# textPos = [.1-(i+1)*spacing,.1,0.011]
# text = "Demo:"+demos[currentDemo][0]+" Frame:"+str(frameNr)+"\nObject UID:"+objectInfo
# textUid = p.addUserDebugText(text,textColorRGB=[0,0,0], textSize = 0.02, textPosition = textPos,textOrientation = textOrn,parentObjectUniqueId=uiCube, parentLinkIndex = -1)
# lines[i] = textUid
#keep the gripper centered/symmetric
b = p.getJointState(pr2_gripper,2)[0]
p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
t6 = mult*time.clock()
events = p.getVREvents()
t7 = mult*time.clock()
#print ("len events=",len(events))
for e in (events):
if e[CONTROLLER_ID] == uiControllerId:
p.resetBasePositionAndOrientation(uiCube,e[POSITION], e[ORIENTATION])
if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED ):
currentDemo = currentDemo+1
if (currentDemo>=len(demos)):
currentDemo = 0
def get_joint_state(body, joint):
return JointState(*p.getJointState(body, joint, physicsClientId=CLIENT))
def getMotorAngles(self):
motorAngles = []
for i in range(self.nMotors):
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
motorAngles.append(jointState[0])
motorAngles = np.multiply(motorAngles, self.motorDir)
return motorAngles
def GetMotorTorques(self):
"""Get the amount of torques the motors are exerting.
Returns:
Motor torques of all eight motors.
"""
if self._accurate_motor_model_enabled or self._pd_control_enabled:
return self._observed_motor_torques
else:
motor_torques = [
p.getJointState(self.minitaur, motor_id)[3] for motor_id in self._motor_id_list ]
motor_torques = np.multiply(motor_torques, self._motor_direction)
return motor_torques
def _getGripper(self):
return pb.getJointState(self.handle, self.left_finger)