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
