def _get_latest_observation(self): """Get observation of the current state. Returns: observation (Observation): the joint positions, velocities, and torques of the joints. """ observation = Observation() current_joint_states = pybullet.getJointStates( self.finger_id, self.pybullet_joint_indices) observation.position = np.array( [joint[0] for joint in current_joint_states]) observation.velocity = np.array( [joint[1] for joint in current_joint_states]) observation.torque = np.array( [joint[3] for joint in current_joint_states]) finger_tip_states = pybullet.getJointStates( self.finger_id, self.pybullet_tip_link_indices) observation.tip_force = np.array( [np.linalg.norm(tip[2][:3]) for tip in finger_tip_states]) # The measurement of the push sensor of the real robot lies in the # interval [0, 1]. It is around 0.23 while there is no contact and # saturates at (very roughly) 20 N. push_sensor_saturation_force_N = 20.0 push_sensor_no_contact_value = 0.23 observation.tip_force /= push_sensor_saturation_force_N observation.tip_force += push_sensor_no_contact_value np.clip(observation.tip_force, 0.0, 1.0, out=observation.tip_force) return observation
def update(self, action): joint_positions = [ state[0] for state in p.getJointStates(self.uid, self.joint_ids) ] joint_velocities = [ state[1] for state in p.getJointStates(self.uid, self.joint_ids) ] n_joints = len(joint_positions) J = p.calculateJacobian(self.uid, self.end_frame, self.offset_admittance_point, joint_positions, [0.] * n_joints, [0.] * n_joints) T_g = p.calculateInverseDynamics(self.uid, joint_positions, [0.] * n_joints, [0.] * n_joints) T_cmd = action['force'].dot(np.array(J[0])) + action['torque'].dot( np.array(J[1])) T_pos = (self.target_pose - np.array(joint_positions)) * self.kp T_vel = (np.array(joint_velocities)) * -self.kd p.setJointMotorControlArray(self.uid, self.joint_ids, p.TORQUE_CONTROL, forces=T_cmd + T_g + T_pos + T_vel)
def _get_obs(self, forces, forces_human): torso_pos = np.array(p.getLinkState(self.robot, 15 if self.robot_type == 'pr2' else 0, computeForwardKinematics=True, physicsClientId=self.id)[0]) state = p.getLinkState(self.tool, 1, computeForwardKinematics=True, physicsClientId=self.id) tool_pos = np.array(state[0]) tool_orient = np.array(state[1]) # Quaternions robot_joint_states = p.getJointStates(self.robot, jointIndices=self.robot_left_arm_joint_indices, physicsClientId=self.id) robot_joint_positions = np.array([x[0] for x in robot_joint_states]) robot_pos, robot_orient = p.getBasePositionAndOrientation(self.robot, physicsClientId=self.id) if self.human_control: human_pos = np.array(p.getBasePositionAndOrientation(self.human, physicsClientId=self.id)[0]) human_joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id) human_joint_positions = np.array([x[0] for x in human_joint_states]) # Human shoulder, elbow, and wrist joint locations 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] robot_obs = np.concatenate([tool_pos-torso_pos, tool_orient, tool_pos - self.target_pos, self.target_pos-torso_pos, robot_joint_positions, shoulder_pos-torso_pos, elbow_pos-torso_pos, wrist_pos-torso_pos, forces]).ravel() if self.human_control: human_obs = np.concatenate([tool_pos-human_pos, tool_orient, tool_pos - self.target_pos, self.target_pos-human_pos, human_joint_positions, shoulder_pos-human_pos, elbow_pos-human_pos, wrist_pos-human_pos, forces_human]).ravel() else: human_obs = [] return np.concatenate([robot_obs, human_obs]).ravel()
def move_to_pos(bodyID, position_matrix): oldJointStates = p.getJointStates(bodyID, [0,1,2]) num_joints = p.getNumJoints(bodyID) precision = .001 distance = 1000 iteration = 0 while (distance > precision and iteration < 50): new_angles = p.calculateInverseKinematics(bodyID, num_joints - 1, position_matrix, residualThreshold = .0001) for joint in range (num_joints - 2): p.setJointMotorControl2(bodyID, joint, p.POSITION_CONTROL, new_angles[joint]) p.stepSimulation() #time.sleep(.005) new_ee_pos = p.getLinkState(bodyID, num_joints - 1)[4] distance = math.sqrt((position_matrix[0] - new_ee_pos[0])**2 + (position_matrix[1] - new_ee_pos[1])**2 + (position_matrix[2] - new_ee_pos[2])**2) iteration = iteration + 1 newJointStates = p.getJointStates(bodyID, [0,1,2]) angularDisplacement = [(newJointStates[0][0] - oldJointStates[0][0]), (newJointStates[1][0] - oldJointStates[1][0]), (newJointStates[2][0] - oldJointStates[2][0])] print("Angular Displacement: " + str(angularDisplacement)) print("Distance: " + str(distance) + " Number of Iterations: " + str(iteration))
def grasp_along(self, axis, pos_tol=0.001, linvel=0.5, force=1000): axis = np.array(axis).flatten() axis = axis / np.linalg.norm(axis) initial_pos = map(lambda joint: joint[0], p.getJointStates(self.gid, range(3))) target_pos = initial_pos + axis * (self.pre_grasp_distance - 0.005) timeout = (self.pre_grasp_distance + 0.01) / linvel # Set target position for joint in range(3): p.setJointMotorControl2(self.gid, joint, p.POSITION_CONTROL, targetPosition=target_pos[joint], targetVelocity=0, force=force, maxVelocity=linvel, positionGain=0.3, velocityGain=1) print 'Moving to grasp point' # Simulation loop for _ in range(int(timeout / self.timestep)): current_pos = map(lambda joint: joint[0], p.getJointStates(self.gid, range(3))) if np.linalg.norm(target_pos - current_pos) < pos_tol: break self.step() print 'Closing gripper' # Close gripper self.close_gripper()
def _get_obs(self): state = [] base_link_info = p.getLinkState(self.walker_id, 3, computeLinkVelocity=1, computeForwardKinematics=1) base_pos = base_link_info[0] base_orn = p.getEulerFromQuaternion(base_link_info[1]) state.append(base_pos[2]) # Z state.append(base_orn[1]) # Pitch for s in p.getJointStates(self.walker_id, self.motor_joints): state.append(s[0]) base_linvel = base_link_info[6] base_angvel = base_link_info[7] state.append(np.clip(base_linvel[1], -10, 10)) # Y state.append(np.clip(base_linvel[2], -10,10)) # Z state.append(np.clip(base_angvel[1], -10,10)) # Pitch for s in p.getJointStates(self.walker_id, self.motor_joints): state.append(np.clip(s[1], -10,10)) return np.array(state)
def enforce_realistic_human_joint_limits(self): # NOTE: Remember human model is different in training and vr # Only enforce limits for the human arm that is moveable (if either arm is even moveable) if 7 in self.human_controllable_joint_indices: # Right human arm tz, tx, ty, qe = [j[0] for j in p.getJointStates(self.human, jointIndices=[7, 8, 9, 10], 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([7, 8, 9, 10]): p.resetJointState(self.human, jointIndex=j, targetValue=self.right_arm_previous_valid_pose[i], targetVelocity=0, physicsClientId=self.id) if 17 in self.human_controllable_joint_indices: # Left human arm tz, tx, ty, qe = [j[0] for j in p.getJointStates(self.human, jointIndices=[17, 18, 19, 20], 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([17, 18, 19, 20]): p.resetJointState(self.human, jointIndex=j, targetValue=self.left_arm_previous_valid_pose[i], targetVelocity=0, physicsClientId=self.id)
def setup_human_joints(self, human, joints_positions, controllable_joints, use_static_joints=True, human_reactive_force=None, human_reactive_gain=0.05): if self.human_impairment != 'tremor': self.human_tremors = np.zeros(len(controllable_joints)) elif len(controllable_joints) == 4: self.human_tremors = self.np_random.uniform(np.deg2rad(-20), np.deg2rad(20), size=len(controllable_joints)) else: self.human_tremors = self.np_random.uniform(np.deg2rad(-10), np.deg2rad(10), size=len(controllable_joints)) # Set starting joint positions human_joint_states = p.getJointStates(human, jointIndices=list(range(p.getNumJoints(human, physicsClientId=self.id))), physicsClientId=self.id) human_joint_positions = np.array([x[0] for x in human_joint_states]) for j in range(p.getNumJoints(human, physicsClientId=self.id)): set_position = None for j_index, j_angle in joints_positions: if j == j_index: p.resetJointState(human, jointIndex=j, targetValue=j_angle, targetVelocity=0, physicsClientId=self.id) set_position = j_angle break if use_static_joints and j not in controllable_joints: # Make all non controllable joints on the person static by setting mass of each link (joint) to 0 p.changeDynamics(human, j, mass=0, physicsClientId=self.id) # Set velocities to 0 p.resetJointState(human, jointIndex=j, targetValue=human_joint_positions[j] if set_position is None else set_position, targetVelocity=0, physicsClientId=self.id) # By default, all joints have motors enabled by default that prevent free motion. Disable these motors in human for j in range(p.getNumJoints(human, physicsClientId=self.id)): p.setJointMotorControl2(human, jointIndex=j, controlMode=p.VELOCITY_CONTROL, force=0, physicsClientId=self.id) self.enforce_joint_limits(human) if human_reactive_force is not None: # NOTE: This runs a Position / Velocity PD controller for each joint motor on the human human_joint_states = p.getJointStates(human, jointIndices=controllable_joints, physicsClientId=self.id) target_human_joint_positions = np.array([x[0] for x in human_joint_states]) forces = [human_reactive_force * self.human_strength] * len(target_human_joint_positions) p.setJointMotorControlArray(human, jointIndices=controllable_joints, controlMode=p.POSITION_CONTROL, targetPositions=target_human_joint_positions, positionGains=np.array([human_reactive_gain]*len(forces)), forces=forces, physicsClientId=self.id)
def arm_sim(self, event, mode): target_pos = event[1] (roll, pitch, yaw) = p.getEulerFromQuaternion(event[2], physicsClientId=self.id) if mode == "left": target_orient = p.getQuaternionFromEuler([-roll, -pitch, yaw-np.deg2rad(180)], physicsClientId=self.id) new_pos, new_orient = p.multiplyTransforms(target_pos, target_orient, [0, 0, 0.08], [0, 0, 0, 1], physicsClientId=self.id) old_pos, old_orient = p.getLinkState(self.left_arm, 6, computeForwardKinematics=True, physicsClientId=self.id)[:2] if np.linalg.norm(np.array(old_pos) - np.array(new_pos)) < 0.001 or np.linalg.norm(np.array(old_orient) - np.array(new_orient)) < 0.01: pass else: targetPositions = self.util.ik_human(body=self.left_arm, target_joint=6, target_pos=new_pos, target_orient=new_orient) p.setJointMotorControlArray(self.human, jointIndices=list(range(17, 24)), controlMode=p.POSITION_CONTROL, targetPositions=targetPositions, positionGains=np.array([self.human_gains]*7), forces=[self.human_forces]*7, physicsClientId=self.id) left_arm_joint_states = p.getJointStates(self.human, jointIndices=list(range(17, 24)), physicsClientId=self.id) left_arm_joint_positions = np.array([x[0] for x in left_arm_joint_states]) for i in range(7): p.resetJointState(self.left_arm, jointIndex=i, targetValue=left_arm_joint_positions[i], targetVelocity=0, physicsClientId=self.id) else: target_orient = p.getQuaternionFromEuler([-roll, -pitch, yaw+np.deg2rad(180)], physicsClientId=self.id) new_pos, new_orient = p.multiplyTransforms(target_pos, target_orient, [0, 0, 0.08], [0, 0, 0, 1], physicsClientId=self.id) old_pos, old_orient = p.getLinkState(self.right_arm, 6, computeForwardKinematics=True, physicsClientId=self.id)[:2] if np.linalg.norm(np.array(old_pos) - np.array(new_pos)) < 0.001 or np.linalg.norm(np.array(old_orient) - np.array(new_orient)) < 0.01: pass else: targetPositions = self.util.ik_human(body=self.right_arm, target_joint=6, target_pos=new_pos, target_orient=new_orient) p.setJointMotorControlArray(self.human, jointIndices=list(range(7, 14)), controlMode=p.POSITION_CONTROL, targetPositions=targetPositions, positionGains=np.array([self.human_gains]*7), forces=[self.human_forces]*7, physicsClientId=self.id) right_arm_joint_states = p.getJointStates(self.human, jointIndices=list(range(7, 14)), physicsClientId=self.id) right_arm_joint_positions = np.array([x[0] for x in right_arm_joint_states]) for i in range(7): p.resetJointState(self.right_arm, jointIndex=i, targetValue=right_arm_joint_positions[i], targetVelocity=0, physicsClientId=self.id)
def get_state(self, form='flat'): if form == 'dict_like': arm_state = {} arm_state['joint_poss'] = bullet.getJointStates( self.id, self.config.arm_joint_indices) gripper_state = {} gripper_state['joint_poss'] = bullet.getJointStates( self.id, self.config.gripper_joint_indices) ee_idx = self.config.ordered_links.index('ee_link') ee_state = bullet.getLinkState(self.id, ee_idx) end_effector_state = {} end_effector_state['link_world_pos'] = ee_state[0] end_effector_state['link_world_orn'] = ee_state[1] return { 'arm': arm_state, 'gripper': gripper_state, 'end_effector': end_effector_state } else: # form == 'flat' arm_state = bullet.getJointStates(self.id, self.config.arm_joint_indices) arm_joint_poss = [state[0] for state in arm_state] arm_joint_vels = [state[1] for state in arm_state] arm_joint_rfs = [rf for state in arm_state for rf in state[2] ] # 6-dim refaction force for each joint ee_idx = self.config.ordered_links.index('ee_link') ee_state = bullet.getLinkState(self.id, ee_idx) ee_pos = [e for e in ee_state[0]] state = np.array(ee_pos + arm_joint_poss + arm_joint_vels + arm_joint_rfs) state = np.append(state, self.prev_action) return state
def update_encoders(self): """ Get encoder states of joints """ if self.en_sway: # directly accessing joint states instead of calculating transforms states = p.getJointStates( bodyUniqueId=self.crane_body_id, jointIndices=[ self._joint_map["hub_to_gantry"], self._joint_map["gantry_to_trolley"], self._joint_map["trolley_to_sway_x"], self._joint_map["sway_x_to_sway_y"], self._joint_map["sway_y_to_hoist"], self._joint_map["grapple_base_to_grapple_body"], self._joint_map["grapple_body_to_left_grapple_jaw"], self._joint_map["grapple_body_to_right_grapple_jaw"], self._joint_map["tracks_to_gantry"], ]) gantry_position = states[0][0] trolley_position = states[1][0] hoist_position = states[4][0] sway_angle_x = states[2][0] sway_angle_y = states[3][0] grapple_base_rotation = states[5][0] % (2 * np.pi) tines_rotation = states[6][0] hub_pos = states[8][0] print("hub angle: {}".format(hub_pos)) print("Gantry angle: {}".format(gantry_position)) print("Trolley pos: {}".format(trolley_position)) print("Sway angle X: {}".format(sway_angle_x)) print("Sway angle Y: {}".format(sway_angle_y)) print("Hoist pos: {}".format(hoist_position)) print("Grapple rotation angle: {}".format(grapple_base_rotation)) print("Tines angle: {}".format(tines_rotation)) else: states = p.getJointStates( bodyUniqueId=self.crane_body_id, jointIndices=[ self._joint_map["hub_to_gantry"], self._joint_map["gantry_to_trolley"], self._joint_map["trolley_to_hoist"], self._joint_map["grapple_base_to_grapple_body"], self._joint_map["grapple_body_to_left_grapple_jaw"], self._joint_map["grapple_body_to_right_grapple_jaw"], ]) gantry_position = states[0][0] trolley_position = states[1][0] hoist_position = states[4][0] grapple_base_rotation = states[3][0] % (2 * np.pi) tines_rotation = states[5][0] print("Gantry angle: {}".format(gantry_position)) print("Trolley pos: {}".format(trolley_position)) print("Hoist pos: {}".format(hoist_position)) print("Grapple rotation angle: {}".format(grapple_base_rotation)) print("Tines angle: {}".format(tines_rotation))
def getJointPositions(self, leg): if np.sum(leg == self.R) == len(leg): jointStates = pb.getJointStates(self._robotId, jointIndices=self._jointIdListR) jointPositions = [jointStates[i][0] for i in range(len(jointStates))] elif np.sum(leg == self.L) == len(leg): jointStates = pb.getJointStates(self._robotId, jointIndices=self._jointIdListL) jointPositions = [jointStates[i][0] for i in range(len(jointStates))] else: raise ValueError("invalid parameter") return jointPositions
def pybulletRendering(initTorques, color): duration = 0 end_effector_xyz_start = [] end_effector_xyz_end = [] lineLength = 2 while duration < 7 * (trajectoryLen - 1): if (duration % 7) == 0: #print(duration/7, " Wait to Receive MPC Control") joint_torques, stop = pybulletServer.recvControl() if stop == True: pybulletServer.responseCurrentState([]) break joint_positions = [ state[0] for state in pybullet.getJointStates( robot, range(pybullet.getNumJoints(robot))) ] joint_velocities = [ state[1] for state in pybullet.getJointStates( robot, range(pybullet.getNumJoints(robot))) ] if duration % (7 * lineLength) == 0: end_effector_xyz_start = pybullet.getLinkState(robot, 7)[4] pybullet.setJointMotorControlArray(robot, range(totoalJointNum), pybullet.TORQUE_CONTROL, forces=[0, 0] + joint_torques + [0, 0]) pybullet.stepSimulation() if (duration % 7) == 6: joint_positions = [ state[0] for state in pybullet.getJointStates( robot, range(pybullet.getNumJoints(robot))) ] joint_velocities = [ state[1] for state in pybullet.getJointStates( robot, range(pybullet.getNumJoints(robot))) ] pybulletServer.responseCurrentState( joint_positions[jointStartIndex:jointStartIndex + jointNum] + joint_velocities[jointStartIndex:jointStartIndex + jointNum]) if duration % (7 * lineLength) == 6: end_effector_xyz_end = pybullet.getLinkState(robot, 7)[4] pybullet.addUserDebugLine(end_effector_xyz_start, end_effector_xyz_end, color, lineWidth=2) duration += 1
def _get_obs(self, forces, forces_human): torso_pos = np.array( p.getLinkState(self.robot, 15 if self.robot_type == 'pr2' else 0, computeForwardKinematics=True, physicsClientId=self.id)[0]) spoon_pos, spoon_orient = p.getBasePositionAndOrientation( self.spoon, physicsClientId=self.id) robot_right_joint_states = p.getJointStates( self.robot, jointIndices=self.robot_right_arm_joint_indices, physicsClientId=self.id) robot_right_joint_positions = np.array( [x[0] for x in robot_right_joint_states]) robot_pos, robot_orient = p.getBasePositionAndOrientation( self.robot, physicsClientId=self.id) if self.human_control: human_pos = np.array( p.getLinkState(self.human, 3, computeForwardKinematics=True, physicsClientId=self.id)[:2][0]) human_joint_states = p.getJointStates( self.human, jointIndices=[24, 25, 26, 27], physicsClientId=self.id) human_joint_positions = np.array( [x[0] for x in human_joint_states]) head_pos, head_orient = p.getLinkState(self.human, 27, computeForwardKinematics=True, physicsClientId=self.id)[:2] robot_obs = np.concatenate([ spoon_pos - torso_pos, spoon_orient, spoon_pos - self.target_pos, robot_right_joint_positions, head_pos - torso_pos, head_orient, forces ]).ravel() if self.human_control: human_obs = np.concatenate([ spoon_pos - human_pos, spoon_orient, spoon_pos - self.target_pos, human_joint_positions, head_pos - human_pos, head_orient, forces_human ]).ravel() else: human_obs = [] return np.concatenate([robot_obs, human_obs]).ravel()
def get_state(self): # Returns a pinocchio like representation of the q, dq matrixes q = zero(self.nq) dq = zero(self.nv) if not self.useFixedBase: pos, orn = p.getBasePositionAndOrientation(self.robot_id) q[:3] = pos q[3:7] = orn vel, orn = p.getBaseVelocity(self.robot_id) dq[:3] = vel dq[3:6] = orn # Pinocchio assumes base velocity to be in body frame -> rotate. rot = np.array(p.getMatrixFromQuaternion(q[3:7])).reshape((3, 3)) dq[0:3] = rot.T.dot(dq[0:3]) dq[3:6] = rot.T.dot(dq[3:6]) # Query the joint readings. joint_states = p.getJointStates(self.robot_id, self.bullet_joint_ids) if not self.useFixedBase: for i in range(self.nj): q[5 + self.pinocchio_joint_ids[i]] = joint_states[i][0] dq[4 + self.pinocchio_joint_ids[i]] = joint_states[i][1] else: for i in range(self.nj): q[self.pinocchio_joint_ids[i] - 1] = joint_states[i][0] dq[self.pinocchio_joint_ids[i] - 1] = joint_states[i][1] return q, dq
def retrieve_pyb_data(self): """Retrieve the position and orientation of the base in world frame as well as its linear and angular velocities """ # Retrieve data from the simulation self.jointStates = pyb.getJointStates(self.robotId, self.revoluteJointIndices) # State of all joints self.baseState = pyb.getBasePositionAndOrientation(self.robotId) # Position and orientation of the trunk self.baseVel = pyb.getBaseVelocity(self.robotId) # Velocity of the trunk # Joints configuration and velocity vector for free-flyer + 12 actuators self.qmes12 = np.vstack((np.array([self.baseState[0]]).T, np.array([self.baseState[1]]).T, np.array([[state[0] for state in self.jointStates]]).T)) self.vmes12 = np.vstack((np.array([self.baseVel[0]]).T, np.array([self.baseVel[1]]).T, np.array([[state[1] for state in self.jointStates]]).T)) """robotVirtualOrientation = pyb.getQuaternionFromEuler([0, 0, np.pi / 4]) self.qmes12[3:7, 0] = robotVirtualOrientation""" # Add uncertainty to feedback from PyBullet to mimic imperfect measurements """tmp = np.array([pyb.getQuaternionFromEuler(pin.rpy.matrixToRpy( pin.Quaternion(self.qmes12[3:7, 0:1]).toRotationMatrix()) + np.random.normal(0, 0.03, (3,)))]) self.qmes12[3:7, 0] = tmp[0, :] self.vmes12[0:6, 0] += np.random.normal(0, 0.01, (6,))""" return 0
def step(self): p.stepSimulation() time.sleep(1.0 / self.sim_freq) temp = p.getJointStates(self.robotID, jointIndices=self.arm_joints) for i in range(len(self.arm_joints)): self.current_arm_joints_position[i] = temp[i][0] self.current_arm_joints_velocity[i] = temp[i][1]
def state(self): # return the current state in the format of [q, dq] x = np.array([s[:2] for s in p.getJointStates(GP.robot,list(qind))]).T.reshape(-1) # normalize the state making the q1_r,q1_l be in the range of 0~2pi, r, q2_r q2_l be in the range -pi~pi x[[2, 4, 6]] = x[[2, 4, 6]] - ((x[[2, 4, 6]] + np.math.pi)//(2*np.math.pi))*(2*np.math.pi) x[[3, 5]] = x[[3, 5]] % (2*np.math.pi) return x
def get_observation(self): # Create observation state observation = [] observation_lim = [] # Get state of the end-effector link state = p.getLinkState(self.robot_id, self.end_eff_idx, computeLinkVelocity=1, computeForwardKinematics=1, physicsClientId=self._physics_client_id) # ------------------------- # # --- Cartesian 6D pose --- # # ------------------------- # pos = state[0] orn = state[1] observation.extend(list(pos)) observation_lim.extend(list(self._workspace_lim)) # cartesian orientation if self._control_eu_or_quat is 0: euler = p.getEulerFromQuaternion(orn) observation.extend(list(euler)) # roll, pitch, yaw observation_lim.extend(self._eu_lim) else: observation.extend(list(orn)) observation_lim.extend([[-1, 1], [-1, 1], [-1, 1], [-1, 1]]) # --------------------------------- # # --- Cartesian linear velocity --- # # --------------------------------- # if self._include_vel_obs: # standardize by subtracting the mean and dividing by the std vel_std = [0.04, 0.07, 0.03] vel_mean = [0.0, 0.01, 0.0] vel_l = np.subtract(state[6], vel_mean) vel_l = np.divide(vel_l, vel_std) observation.extend(list(vel_l)) observation_lim.extend([[-1, 1], [-1, 1], [-1, 1]]) # ------------------- # # --- Joint poses --- # # ------------------- # jointStates = p.getJointStates(self.robot_id, self._joint_name_to_ids.values(), physicsClientId=self._physics_client_id) jointPoses = [x[0] for x in jointStates] observation.extend(list(jointPoses)) observation_lim.extend( [[self.ll[i], self.ul[i]] for i in range(0, len(self._joint_name_to_ids.values()))]) return observation, observation_lim
def get_obs(self): # Base position and orientation torso_pos, torso_orient = p.getBasePositionAndOrientation(self.robotId) # Target position and orientation target_pos, _ = p.getBasePositionAndOrientation(self.targetId) # Base velocity and angular velocity torso_pos_, torso_orient_ = p.getBaseVelocity(self.robotId) # Joint states and velocities q, q_, _, _ = zip(*p.getJointStates(self.robotId, self.joint_ids)) obs_dict = { 'torso_pos': torso_pos, 'torso_quat': torso_orient, 'target_pos': target_pos[0:2], 'q': q, 'torso_vel': torso_pos_, 'torso_angvel': torso_orient_, 'q_vel': q_ } obs_arr = np.concatenate([v for v in obs_dict.values() ]).astype(np.float32) return obs_arr, obs_dict
def _get_obs(self, forces=None, ret_images=False): fork_pos, fork_orient = p.getBasePositionAndOrientation( self.drop_fork, physicsClientId=self.id) food_pos, food_orient = p.getBasePositionAndOrientation( self.foodItem, physicsClientId=self.id) robot_right_joint_states = p.getJointStates( self.robot, jointIndices=self.robot_right_arm_joint_indices, physicsClientId=self.id) robot_right_joint_positions = np.array( [x[0] for x in robot_right_joint_states]) robot_pos, robot_orient = p.getBasePositionAndOrientation( self.robot, physicsClientId=self.id) # get forces if not precomputed if forces is None: forces = self.get_total_force() # robot, fork, food robot_obs = np.concatenate([ robot_right_joint_positions, fork_pos, # 3D drop fork position fork_orient, # drop fork orientation food_pos, # 3D food position food_orient, # food orientation (absolute) self.food_orient_quat, # food orientation (relative to fork) [self.food_type], # food type self.mouth_pos, # 3D mouth center pos (target) self.mouth_orient, # mouth orientation (absolute) forces ]).ravel().astype(np.float32) if ret_images: return robot_obs, self.depth_opengl1, self.depth_opengl2 return robot_obs
def get_qadot(self): jointStates = p.getJointStates(self.robotId, self.revoluteJointIndices) qadot = np.array([[ jointStates[i_joint][1] for i_joint in range(len(jointStates)) ]]).transpose() return qadot
def get_obs(self): hand_pos = np.array( p.getLinkState(self.pandaUid, pandaJointsDict["panda_grasptarget_hand"])[0]) hand_ori = np.array( p.getLinkState(self.pandaUid, pandaJointsDict["panda_grasptarget_hand"])[1]) obj_pos, obj_ori = p.getBasePositionAndOrientation(self.objectUid) obj_pos = np.array(obj_pos) # rel_pos = fingertip_pos - obj_pos # q pos of contollable / dof joints qpos_joints = np.array((p.getJointStates(self.pandaUid, list(range(7)) + [9, 10])), dtype=object)[:, 0] finger_pos1 = p.getLinkState(self.pandaUid, pandaJointsDict["panda_finger_joint1"])[0] finger_pos2 = p.getLinkState(self.pandaUid, pandaJointsDict["panda_finger_joint2"])[0] finger_pos = np.array([finger_pos1, finger_pos2]) dist_fingers = np.sqrt((obj_pos - finger_pos)**2).reshape([-1]) obs_dict = { "dist_fingers": dist_fingers, "obj_ori": obj_ori, "obj_pos": obj_pos, "palm_ori": hand_ori, "palm_pos": hand_pos, "qpos_joints": qpos_joints, } observation = np.concatenate([v for _, v in sorted(obs_dict.items())]) observation = self.process_observation(observation) return observation, obs_dict
def collect_observations(drone): #print("ordered_joint_indices") #print(ordered_joint_indices) jointStates = p.getJointStates(drone,ordered_joint_indices) j = np.array([current_relative_position(jointStates, drone, *jtuple) for jtuple in ordered_joints]).flatten() #print("j") #print(j) body_xyz, (qx, qy, qz, qw) = p.getBasePositionAndOrientation(drone) #print("body_xyz") #print(body_xyz, qx,qy,qz,qw) z = body_xyz[2] dummy.distance = body_xyz[0] if dummy.initial_z==None: dummy.initial_z = z (vx, vy, vz), _ = p.getBaseVelocity(drone) more = np.array([z-dummy.initial_z, 0.1*vx, 0.1*vy, 0.1*vz, qx, qy, qz, qw]) # rcont = p.getContactPoints(drone, -1, right_foot, -1) # #print("rcont") # #print(rcont) # lcont = p.getContactPoints(drone, -1, left_foot, -1) # #print("lcont") # #print(lcont) # feet_contact = np.array([len(rcont)>0, len(lcont)>0]) # return np.clip( np.concatenate([more] + [j] + [feet_contact]), -5, +5) return np.clip( np.concatenate([more] + [j]), -5, +5)
def enforce_joint_limits(self, body): # Enforce joint limits joint_states = p.getJointStates(body, jointIndices=list(range(p.getNumJoints(body, physicsClientId=self.id))), physicsClientId=self.id) joint_positions = np.array([x[0] for x in joint_states]) lower_limits = [] upper_limits = [] for j in range(p.getNumJoints(body, physicsClientId=self.id)): joint_info = p.getJointInfo(body, j, physicsClientId=self.id) joint_name = joint_info[1] joint_pos = joint_positions[j] lower_limit = joint_info[8] upper_limit = joint_info[9] if lower_limit == 0 and upper_limit == -1: lower_limit = -1e10 upper_limit = 1e10 lower_limits.append(lower_limit) upper_limits.append(upper_limit) # print(joint_name, joint_pos, lower_limit, upper_limit) if joint_pos < lower_limit: p.resetJointState(body, jointIndex=j, targetValue=lower_limit, targetVelocity=0, physicsClientId=self.id) elif joint_pos > upper_limit: p.resetJointState(body, jointIndex=j, targetValue=upper_limit, targetVelocity=0, physicsClientId=self.id) lower_limits = np.array(lower_limits) upper_limits = np.array(upper_limits) return lower_limits, upper_limits
def __safety_check_torques(self, desired_torques): """ Perform a check on the torques being sent to be applied to the motors so that they do not exceed the safety torque limit Args: desired_torques (array): The torques desired to be applied to the motors Returns: applied_torques (array): The torques that can be actually applied to the motors (and will be applied) """ applied_torques = np.clip( np.asarray(desired_torques), -self.max_motor_torque, +self.max_motor_torque, ) current_joint_states = pybullet.getJointStates( self.finger_id, self.pybullet_joint_indices) current_velocity = np.array( [joint[1] for joint in current_joint_states]) applied_torques -= self.safety_kd * current_velocity applied_torques = np.clip( np.asarray(applied_torques), -self.max_motor_torque, +self.max_motor_torque, ) return applied_torques
def collect_observations(self, human): #print("ordered_joint_indices") #print(ordered_joint_indices) jointStates = p.getJointStates(human,self.ordered_joint_indices) j = np.array([self.current_relative_position(jointStates, human, *jtuple) for jtuple in self.ordered_joints]).flatten() #print("j") #print(j) body_xyz, (qx, qy, qz, qw) = p.getBasePositionAndOrientation(human) #print("body_xyz") #print(body_xyz, qx,qy,qz,qw) z = body_xyz[2] self.distance = body_xyz[0] if self.initial_z==None: self.initial_z = z (vx, vy, vz), _ = p.getBaseVelocity(human) more = np.array([z-self.initial_z, 0.1*vx, 0.1*vy, 0.1*vz, qx, qy, qz, qw]) rcont = p.getContactPoints(human, -1, self.right_foot, -1) #print("rcont") #print(rcont) lcont = p.getContactPoints(human, -1, self.left_foot, -1) #print("lcont") #print(lcont) feet_contact = np.array([len(rcont)>0, len(lcont)>0]) return np.clip( np.concatenate([more] + [j] + [feet_contact]), -5, +5)
def collect_observations(self, human): #print("ordered_joint_indices") #print(ordered_joint_indices) jointStates = p.getJointStates(human, self.ordered_joint_indices) j = np.array([ self.current_relative_position(jointStates, human, *jtuple) for jtuple in self.ordered_joints ]).flatten() #print("j") #print(j) body_xyz, (qx, qy, qz, qw) = p.getBasePositionAndOrientation(human) #print("body_xyz") #print(body_xyz, qx,qy,qz,qw) z = body_xyz[2] self.distance = body_xyz[0] if self.initial_z == None: self.initial_z = z (vx, vy, vz), _ = p.getBaseVelocity(human) more = np.array( [z - self.initial_z, 0.1 * vx, 0.1 * vy, 0.1 * vz, qx, qy, qz, qw]) rcont = p.getContactPoints(human, -1, self.right_foot, -1) #print("rcont") #print(rcont) lcont = p.getContactPoints(human, -1, self.left_foot, -1) #print("lcont") #print(lcont) feet_contact = np.array([len(rcont) > 0, len(lcont) > 0]) return np.clip(np.concatenate([more] + [j] + [feet_contact]), -5, +5)
def get_state(self): # Returns a pinocchio like representation of the q, dq matrixes q = zero(self.nq) dq = zero(self.nv) pos, orn = p.getBasePositionAndOrientation(self.robot_id) q[:3, 0] = np.array(pos).reshape(3, 1) q[3:7, 0] = np.array(orn).reshape(4, 1) vel, orn = p.getBaseVelocity(self.robot_id) dq[:3, 0] = np.array(vel).reshape(3, 1) dq[3:6, 0] = np.array(orn).reshape(3, 1) # Pinocchio assumes the base velocity to be in the body frame -> rotate. rot = np.matrix(p.getMatrixFromQuaternion(q[3:7])).reshape((3, 3)) dq[0:3] = rot.T.dot(dq[0:3]) dq[3:6] = rot.T.dot(dq[3:6]) # Query the joint readings. joint_states = p.getJointStates(self.robot_id, self.bullet_joint_ids) for i in range(self.nj): q[5 + self.pinocchio_joint_ids[i], 0] = joint_states[i][0] dq[4 + self.pinocchio_joint_ids[i], 0] = joint_states[i][1] return q, dq
def _getObservation(self): allJoints = [j.value for j in Joints] jointStates = p.getJointStates(self.anymal, allJoints) position = np.array([js[0] for js in jointStates]) velocity = np.array([js[1] for js in jointStates]) global lastVelocity if self.t == 0.0: lastVelocity = velocity acceleration = (velocity - lastVelocity) * SIMULATIONFREQUENCY lastVelocity = velocity basePosition, baseOrientation = p.getBasePositionAndOrientation(self.anymal) baseOrientation_Matrix = np.array(p.getMatrixFromQuaternion(baseOrientation)).reshape(3, 3) baseOrientationVector = np.matmul(baseOrientation_Matrix, [0, 0, -1]) baseVelocity, baseAngularVelocity = p.getBaseVelocity(self.anymal) observationAsArray = np.concatenate([ position, velocity, basePosition, baseOrientation, baseOrientationVector, baseVelocity, baseAngularVelocity, acceleration ]) observationAsDict = { "position": position, "velocity": velocity, "basePosition": basePosition, "baseOrientation": baseOrientation, "baseOrientationVector": baseOrientationVector, "baseVelocity": baseVelocity, "baseAngularVelocity": baseAngularVelocity, "acceleration": acceleration } return observationAsArray, observationAsDict
def motor_joint_velocities(self): """ returns the motor joint positions for "each DoF" according to pb. Note: fixed joints have 0 degrees of freedoms. """ joint_states = pb.getJointStates( self._pb_sawyer, range(pb.getNumJoints(self._pb_sawyer, physicsClientId=self._clid)), physicsClientId=self._clid) # Joint info specifies type of joint ("fixed" or not) joint_infos = [ pb.getJointInfo(self._pb_sawyer, i, physicsClientId=self._clid) for i in range( pb.getNumJoints(self._pb_sawyer, physicsClientId=self._clid)) ] # Only get joint states of free joints joint_states = [ j for j, i in zip(joint_states, joint_infos) if i[2] != pb.JOINT_FIXED ] joint_velocities = [state[1] for state in joint_states] return joint_velocities
def getMotorJointStates(robot): joint_states = p.getJointStates(robot, range(p.getNumJoints(robot))) joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))] joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1] joint_positions = [state[0] for state in joint_states] joint_velocities = [state[1] for state in joint_states] joint_torques = [state[3] for state in joint_states] return joint_positions, joint_velocities, joint_torques
q_vel_desired[0][s] = math.cos(2. * math.pi * t[s]) - 1. q_vel_desired[1][s] = math.sin(2. * math.pi * t[s]) q_acc_desired[0][s] = -2. * math.pi * math.sin(2. * math.pi * t[s]) q_acc_desired[1][s] = 2. * math.pi * math.cos(2. * math.pi * t[s]) q_pos = [[0.]* steps,[0.]* steps] q_vel = [[0.]* steps,[0.]* steps] q_tor = [[0.]* steps,[0.]* steps] # Do Torque Control: for i in range(len(t)): # Read Sensor States: joint_states = bullet.getJointStates(id_robot, id_revolute_joints) q_pos[0][i] = joint_states[0][0] a = joint_states[1][0] if (verbose): print("joint_states[1][0]") print(joint_states[1][0]) q_pos[1][i] = a q_vel[0][i] = joint_states[0][1] q_vel[1][i] = joint_states[1][1] # Computing the torque from inverse dynamics: obj_pos = [q_pos[0][i], q_pos[1][i]] obj_vel = [q_vel[0][i], q_vel[1][i]] obj_acc = [q_acc_desired[0][i], q_acc_desired[1][i]]
def getJointStates(robot): joint_states = p.getJointStates(robot, range(p.getNumJoints(robot))) joint_positions = [state[0] for state in joint_states] joint_velocities = [state[1] for state in joint_states] joint_torques = [state[3] for state in joint_states] return joint_positions, joint_velocities, joint_torques