def __init__(self, ):
rospy.on_shutdown(self.clean_shutdown)
self._rate = 500.0
self._right_arm = intera_interface.limb.Limb("right")
self._right_joint_names = self._right_arm.joint_names()
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self.controller = SawyerEEFVelocityController()
self._right_arm.move_to_neutral(speed=0.1)
# Getting the base rotation in eef for coordinate frame transformations
eef_pose_in_base = self._right_arm.endpoint_pose()
self.eef_quat_in_base = np.array([
eef_pose_in_base['orientation'].x,
eef_pose_in_base['orientation'].y,
eef_pose_in_base['orientation'].z,
eef_pose_in_base['orientation'].w,
])
self.eef_rot_in_base = T.quat2mat(self.eef_quat_in_base)
self.base_rot_in_eef = self.eef_rot_in_base.T
def __init__(self, right_arm):
# Make sure there is a clean shutdown
rospy.on_shutdown(self.clean_shutdown)
# control parameters
self._rate = 500.0 # Hz
self._right_arm = right_arm
self._right_joint_names = self._right_arm.joint_names()
self.set_neutral()
# end-effector initial pose
eef_pose = self._right_arm.endpoint_pose()
self.start_eef_pos = np.array([eef_pose['position'].x,
eef_pose['position'].y,
eef_pose['position'].z,
])
self.start_eef_quat = np.array([eef_pose['orientation'].x,
eef_pose['orientation'].y,
eef_pose['orientation'].z,
eef_pose['orientation'].w, ])
self.base_rot_in_eef = T.quat2mat(self.start_eef_quat).T
self.start_eef_pos = self.calibrate_initial_position()
# Eef velocity control
self.controller = SawyerEEFVelocityController()
# Sin-cos control
self.period_factor = 0.15
self.amplitude_factor = 0.1
def __init__(self, right_arm, reset_goal_pos=False, predefined_goal=None):
'''
Creates observations for the Reaching task
:param right_arm: The intera link object to control the robot.
:param reset_goal_pos: Whether to reset any pre-existing goal position
:param predefined_goal: Use one of the hard coded goal positions to avoid placing the end effector at the goal by hand.
'''
self._right_arm = right_arm
self._joint_names = self._right_arm.joint_names()
self.previous_joint_vels_array = np.zeros((7, ))
self._right_arm.move_to_neutral(speed=0.1)
# Getting the base rotation in eef for coordinate frame transformations
eef_pose_in_base = self._right_arm.endpoint_pose()
self.eef_quat_in_base = np.array([
eef_pose_in_base['orientation'].x,
eef_pose_in_base['orientation'].y,
eef_pose_in_base['orientation'].z,
eef_pose_in_base['orientation'].w,
])
self.eef_rot_in_base = T.quat2mat(self.eef_quat_in_base)
self.base_rot_in_eef = self.eef_rot_in_base.T
self.controller = SawyerEEFVelocityController()
# Predifined, hard-coded, goal positions for fast switching between pre-defined goals.
# These vectors correspond to the different positions of eefg in the base frame.
self.predefined_goals = {
'goal_1':
np.array([
0.44889998342216186, 0.15767033384085796, -0.07223470331644867
]),
'goal_2':
np.array(
[0.4064366403627939, 0.2151227875993398,
-0.07152062349980176]),
'goal_3':
np.array(
[0.5353834307301527, 0.0993579385905306, -0.072644653035373])
}
if (predefined_goal is None):
self.goal_pos_in_base = self.record_goal_pos(reset_goal_pos)
elif predefined_goal in self.predefined_goals:
self.set_predefined_goal(predefined_goal)
else:
raise NotImplementedError()
def __init__(self):
# Make sure there is a clean shutdown
rospy.on_shutdown(self.clean_shutdown)
# Setting up Sawyer
self._right_arm = intera_interface.limb.Limb("right")
self._right_joint_names = self._right_arm.joint_names()
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self.set_neutral()
# end-effector initial pose
self.eef_pose = self._right_arm.endpoint_pose()
self.eef_quat = np.array([
self.eef_pose['orientation'].x,
self.eef_pose['orientation'].y,
self.eef_pose['orientation'].z,
self.eef_pose['orientation'].w,
])
self.base_rot_in_eef = T.quat2mat(self.eef_quat).T
# control parameters
self._rate = 500.0 # Hz
# Eef velocity control
self.controller = SawyerEEFVelocityController()
#Policy parameters
self.policy_type = 'joint'
self.line_policy = np.array([0., 0., 0.])
self.period_factor = 0.15
self.amplitude_factor = 0.1
class Control(object):
def __init__(self, right_arm):
# Make sure there is a clean shutdown
rospy.on_shutdown(self.clean_shutdown)
# control parameters
self._rate = 500.0 # Hz
self._right_arm = right_arm
self._right_joint_names = self._right_arm.joint_names()
self.set_neutral()
# end-effector initial pose
eef_pose = self._right_arm.endpoint_pose()
self.start_eef_pos = np.array([eef_pose['position'].x,
eef_pose['position'].y,
eef_pose['position'].z,
])
self.start_eef_quat = np.array([eef_pose['orientation'].x,
eef_pose['orientation'].y,
eef_pose['orientation'].z,
eef_pose['orientation'].w, ])
self.base_rot_in_eef = T.quat2mat(self.start_eef_quat).T
self.start_eef_pos = self.calibrate_initial_position()
# Eef velocity control
self.controller = SawyerEEFVelocityController()
# Sin-cos control
self.period_factor = 0.15
self.amplitude_factor = 0.1
def calibrate_initial_position(self):
''' Set the starting position of the end-effector.'''
rospy.loginfo('Calibrating reaching starting position....')
if (rospy.has_param('start_reaching_position')):
return np.array(rospy.get_param('start_reaching_position'))
else:
rospy.set_param('manual_arm_move', True)
rate = rospy.Rate(20)
while True:
if(not rospy.get_param('manual_arm_move')):
rospy.loginfo('Done')
return self.done_moving()
rate.sleep()
def done_moving(self):
''' Record the current pose of the end effector and end the manual moving.'''
rospy.loginfo('Recording start pos')
eef_pose = self._right_arm.endpoint_pose()
start_eef_pos = np.array([eef_pose['position'].x,
eef_pose['position'].y,
eef_pose['position'].z,
])
rospy.set_param('start_reaching_position', start_eef_pos.tolist())
return start_eef_pos
def _reset_control_modes(self):
rate = rospy.Rate(self._rate)
for _ in xrange(100):
if rospy.is_shutdown():
return False
self._right_arm.exit_control_mode()
rate.sleep()
return True
def get_right_hand_quat(self):
eef_pose = self._right_arm.endpoint_pose()
return np.array([eef_pose['orientation'].x,
eef_pose['orientation'].y,
eef_pose['orientation'].z,
eef_pose['orientation'].w, ])
def get_right_hand_pos(self):
eef_pose = self._right_arm.endpoint_pose()
return np.array([eef_pose['position'].x,
eef_pose['position'].y,
eef_pose['position'].z, ])
def go_to_pose(self, position, orientation):
'''Send the end effector to a specified pose'''
try:
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=self._right_arm)
wpt_opts = MotionWaypointOptions(max_linear_speed=0.6,
max_linear_accel=0.6,
max_rotational_speed=1.57,
max_rotational_accel=1.57,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=self._right_arm)
pose = Pose()
pose.position.x = position[0]
pose.position.y = position[1]
pose.position.z = position[2]
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[0]
poseStamped = PoseStamped()
poseStamped.pose = pose
joint_angles = self._right_arm.joint_ordered_angles()
waypoint.set_cartesian_pose(poseStamped, "right_hand", joint_angles)
rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string())
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=10)
if result is None:
rospy.logerr('Trajectory FAILED to send')
return
if result.result:
rospy.loginfo('Motion controller successfully finished the trajectory!')
else:
rospy.logerr('Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.')
def go_to_start(self):
self.go_to_pose(self.start_eef_pos, self.start_eef_quat)
def set_neutral(self):
"""
Sets both arms back into a neutral pose.
"""
print("Moving to neutral pose...")
self._right_arm.move_to_neutral(speed = 0.15)
def clean_shutdown(self):
print("\nExiting example...")
# return to normal
self._reset_control_modes()
self.set_neutral()
return True
def get_control(self, action, max_action = 0.1, safety_max = 0.15):
'''Convert the action of joint velocity commands.
Use inverse kinematics to go from end effector cartesian velocities to joint velocities.'''
action = max_action * action
current_joint_angles = [self._right_arm.joint_angles()['right_j{}'.format(i)] for i in range(7)]
# get x,y,z velocity in base frame
action_in_base = self.base_rot_in_eef.dot(action)
action_in_base = np.clip(action_in_base, -safety_max, safety_max)
# Correct for any change in orientation using a proportional controller
current_right_hand_quat = self.get_right_hand_quat()
reference_right_hand_quat = self.start_eef_quat
orn_diff = T.quat_multiply(reference_right_hand_quat, T.quat_inverse(current_right_hand_quat))
orn_diff_mat = T.quat2mat(orn_diff)
orn_diff_twice = orn_diff_mat.dot(orn_diff_mat)
# Construct pose matrix
pose_matrix = np.zeros((4, 4))
pose_matrix[:3, :3] = orn_diff_twice
pose_matrix[:3, 3] = action_in_base
pose_matrix[3, 3] = 1
# pose_matrix = T.pose2mat((action_in_base, orn_diff))
joint_velocities = self.controller.compute_joint_velocities_for_endpoint_velocity(pose_matrix,
current_joint_angles)
j_v = {}
for i in range(7):
key = 'right_j{}'.format(i)
j_v[key] = joint_velocities[i]
return j_v
def get_joints_vel(self):
joints_vel = np.array([
self._right_arm.joint_velocity('right_j0'),
self._right_arm.joint_velocity('right_j1'),
self._right_arm.joint_velocity('right_j2'),
self._right_arm.joint_velocity('right_j3'),
self._right_arm.joint_velocity('right_j4'),
self._right_arm.joint_velocity('right_j5'),
self._right_arm.joint_velocity('right_j6'),
])
return joints_vel
def get_joints_angle(self):
joints_angle = np.array([
self._right_arm.joint_velocity('right_j0'),
self._right_arm.joint_velocity('right_j1'),
self._right_arm.joint_velocity('right_j2'),
self._right_arm.joint_velocity('right_j3'),
self._right_arm.joint_velocity('right_j4'),
self._right_arm.joint_velocity('right_j5'),
self._right_arm.joint_velocity('right_j6'),
])
return joints_angle
def joint_array_from_joint_dict(self, joint_dict):
joints_array = np.array([
joint_dict['right_j0'],
joint_dict['right_j1'],
joint_dict['right_j2'],
joint_dict['right_j3'],
joint_dict['right_j4'],
joint_dict['right_j5'],
joint_dict['right_j6'],
])
return joints_array.squeeze()
class ManualPositioning(object):
def __init__(self, ):
rospy.on_shutdown(self.clean_shutdown)
self._rate = 500.0
self._right_arm = intera_interface.limb.Limb("right")
self._right_joint_names = self._right_arm.joint_names()
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self.controller = SawyerEEFVelocityController()
self._right_arm.move_to_neutral(speed=0.1)
# Getting the base rotation in eef for coordinate frame transformations
eef_pose_in_base = self._right_arm.endpoint_pose()
self.eef_quat_in_base = np.array([
eef_pose_in_base['orientation'].x,
eef_pose_in_base['orientation'].y,
eef_pose_in_base['orientation'].z,
eef_pose_in_base['orientation'].w,
])
self.eef_rot_in_base = T.quat2mat(self.eef_quat_in_base)
self.base_rot_in_eef = self.eef_rot_in_base.T
def get_right_hand_quat(self):
eef_pose = self._right_arm.endpoint_pose()
return np.array([
eef_pose['orientation'].x,
eef_pose['orientation'].y,
eef_pose['orientation'].z,
eef_pose['orientation'].w,
])
def get_right_hand_pos(self):
eef_pose = self._right_arm.endpoint_pose()
return np.array([
eef_pose['position'].x,
eef_pose['position'].y,
eef_pose['position'].z,
])
def get_control(self, action, max_action=0.001, safety_max=0.1):
action = max_action * action
current_joint_angles = [
self._right_arm.joint_angles()['right_j{}'.format(i)]
for i in range(7)
]
# get x,y,z velocity in base frame
action_in_base = self.base_rot_in_eef.dot(action)
action_in_base = np.clip(action_in_base, -safety_max, safety_max)
# Correct for any change in orientation using a proportional controller
current_right_hand_quat = self.get_right_hand_quat()
reference_right_hand_quat = self.eef_quat_in_base
orn_diff = T.quat_multiply(reference_right_hand_quat,
T.quat_inverse(current_right_hand_quat))
orn_diff_mat = T.quat2mat(orn_diff)
orn_diff_twice = orn_diff_mat.dot(orn_diff_mat)
# Construct pose matrix
pose_matrix = np.zeros((4, 4))
pose_matrix[:3, :3] = orn_diff_twice
pose_matrix[:3, 3] = action_in_base
pose_matrix[3, 3] = 1
# pose_matrix = T.pose2mat((action_in_base, orn_diff))
joint_velocities = self.controller.compute_joint_velocities_for_endpoint_velocity(
pose_matrix, current_joint_angles)
j_v = {}
for i in range(7):
key = 'right_j{}'.format(i)
j_v[key] = joint_velocities[i]
return j_v
def move_eef(self):
action_size = 0.05
rate = rospy.Rate(100)
action = np.array([0.0, 0.0, 0.0])
print('Move the end-effector and press d for done')
print('''\r **Ready. Do one of the following.
8 : go forwards in x
2 : go backwards in x
6 : go forwards in y
4 : go backwards in y
+ : go forwards in z
- : go backwards in z
h : double speed
l : half speed
d : finish moving
esc : quit
''')
while not rospy.is_shutdown():
c = intera_external_devices.getch()
if c:
if (c in ['8']):
action = np.array([1.0, 0.0, 0.0])
elif (c in ['2']):
action = np.array([-1.0, 0.0, 0.0])
elif (c in ['6']):
action = np.array([0.0, 1.0, 0.0])
elif (c in ['4']):
action = np.array([0.0, -1.0, 0.0])
elif (c in ['+']):
action = np.array([0.0, 0.0, 1.0])
elif (c in ['-']):
action = np.array([0.0, 0.0, -1.0])
elif (c in ['5']):
action = np.array([0.0, 0.0, 0.0])
elif (c in ['h']):
action = np.array([0.0, 0.0, 0.0])
action_size = 2 * action_size
elif (c in ['l']):
action = np.array([0.0, 0.0, 0.0])
action_size = action_size / 2
elif c in ['\x1b', '\x03', 'f']:
action = np.array([0.0, 0.0, 0.0])
rospy.signal_shutdown("Shutting Down...")
elif c in ['d']:
rospy.loginfo('Done Moving')
rospy.set_param('manual_arm_move', False)
print(self._right_arm.endpoint_pose())
return
joint_vels = self.get_control(action, max_action=action_size)
# Send contgrol to robot
self._right_arm.set_joint_velocities(joint_vels)
rate.sleep()
return
def clean_shutdown(self):
print("\nExiting example...")
# return to normal
self._reset_control_modes()
return True
def _reset_control_modes(self):
rate = rospy.Rate(self._rate)
for _ in xrange(100):
if rospy.is_shutdown():
return False
self._right_arm.exit_control_mode()
rate.sleep()
def __init__(self):
# Make sure there is a clean shutdown
rospy.on_shutdown(self.clean_shutdown)
# control parameters
self._rate = 500.0 # Hz
# Setting up the observation subscriber
self.characterisation_observation = None
self.observation = None
self.characterisation_observation_subscriber = rospy.Subscriber('characterisation_observations',
pushing_characterisation_observations,
self.characterisation_observation_callback,
queue_size=1)
self.observation_subscriber = rospy.Subscriber('observations', numpy_msg(Floats64),
self.observation_callback,
queue_size=1)
while True:
if (self.characterisation_observation is not None and self.observation is not None):
break
# Setting up Sawyer
self._right_arm = intera_interface.limb.Limb("right")
self._right_joint_names = self._right_arm.joint_names()
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self.set_neutral()
# end-effector initial pose
eef_pose = self._right_arm.endpoint_pose()
self.start_eef_pos = np.array([eef_pose['position'].x,
eef_pose['position'].y,
eef_pose['position'].z,
])
self.start_eef_quat = np.array([eef_pose['orientation'].x,
eef_pose['orientation'].y,
eef_pose['orientation'].z,
eef_pose['orientation'].w, ])
self.base_rot_in_eef = T.quat2mat(self.start_eef_quat).T
if (not rospy.has_param('pushing/start_position')):
self.start_eef_pos, self.start_eef_quat = self.calibrate_initial_position()
else:
self.start_eef_pos = np.array(rospy.get_param('pushing/start_position'))
self.start_eef_quat = np.array(rospy.get_param('pushing/start_orientation'))
# Eef velocity control
self.controller = SawyerEEFVelocityController()
# Sin-cos control
self.period_factor = 0.15
self.amplitude_factor = 0.1
class Control(object):
def __init__(self):
# Make sure there is a clean shutdown
rospy.on_shutdown(self.clean_shutdown)
# control parameters
self._rate = 500.0 # Hz
# Setting up the observation subscriber
self.characterisation_observation = None
self.observation = None
self.characterisation_observation_subscriber = rospy.Subscriber('characterisation_observations',
pushing_characterisation_observations,
self.characterisation_observation_callback,
queue_size=1)
self.observation_subscriber = rospy.Subscriber('observations', numpy_msg(Floats64),
self.observation_callback,
queue_size=1)
while True:
if (self.characterisation_observation is not None and self.observation is not None):
break
# Setting up Sawyer
self._right_arm = intera_interface.limb.Limb("right")
self._right_joint_names = self._right_arm.joint_names()
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self.set_neutral()
# end-effector initial pose
eef_pose = self._right_arm.endpoint_pose()
self.start_eef_pos = np.array([eef_pose['position'].x,
eef_pose['position'].y,
eef_pose['position'].z,
])
self.start_eef_quat = np.array([eef_pose['orientation'].x,
eef_pose['orientation'].y,
eef_pose['orientation'].z,
eef_pose['orientation'].w, ])
self.base_rot_in_eef = T.quat2mat(self.start_eef_quat).T
if (not rospy.has_param('pushing/start_position')):
self.start_eef_pos, self.start_eef_quat = self.calibrate_initial_position()
else:
self.start_eef_pos = np.array(rospy.get_param('pushing/start_position'))
self.start_eef_quat = np.array(rospy.get_param('pushing/start_orientation'))
# Eef velocity control
self.controller = SawyerEEFVelocityController()
# Sin-cos control
self.period_factor = 0.15
self.amplitude_factor = 0.1
def calibrate_initial_position(self):
rospy.loginfo('Place the end-effector in the desired starting position and press d...')
if (rospy.has_param('pushing/start_position')):
return np.array(rospy.get_param('pushing/start_position'))
else:
rospy.set_param('manual_arm_move', True)
rate = rospy.Rate(20)
while True:
if (not rospy.get_param('manual_arm_move')):
rospy.loginfo('Done')
return self.done_moving()
rate.sleep()
def done_moving(self):
eef_pose = self._right_arm.endpoint_pose()
start_eef_pos = np.array([eef_pose['position'].x,
eef_pose['position'].y,
eef_pose['position'].z,
])
start_eef_quat = np.array([eef_pose['orientation'].x,
eef_pose['orientation'].y,
eef_pose['orientation'].z,
eef_pose['orientation'].w, ])
rospy.set_param('pushing/start_position', start_eef_pos.tolist())
rospy.set_param('pushing/start_orientation', start_eef_quat.tolist())
return start_eef_pos, start_eef_quat
def _reset_control_modes(self):
rate = rospy.Rate(self._rate)
for _ in xrange(100):
if rospy.is_shutdown():
return False
self._right_arm.exit_control_mode()
rate.sleep()
return True
def get_right_hand_quat(self):
eef_pose = self._right_arm.endpoint_pose()
return np.array([eef_pose['orientation'].x,
eef_pose['orientation'].y,
eef_pose['orientation'].z,
eef_pose['orientation'].w, ])
def get_right_hand_pos(self):
eef_pose = self._right_arm.endpoint_pose()
return np.array([eef_pose['position'].x,
eef_pose['position'].y,
eef_pose['position'].z, ])
def go_to_pose(self, position, orientation):
try:
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=self._right_arm)
wpt_opts = MotionWaypointOptions(max_linear_speed=0.6,
max_linear_accel=0.6,
max_rotational_speed=1.57,
max_rotational_accel=1.57,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=self._right_arm)
pose = Pose()
pose.position.x = position[0]
pose.position.y = position[1]
pose.position.z = position[2]
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[0]
poseStamped = PoseStamped()
poseStamped.pose = pose
joint_angles = self._right_arm.joint_ordered_angles()
waypoint.set_cartesian_pose(poseStamped, "right_hand", joint_angles)
rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string())
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=10)
if result is None:
rospy.logerr('Trajectory FAILED to send')
return
if result.result:
rospy.loginfo('Motion controller successfully finished the trajectory!')
else:
rospy.logerr('Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.')
def go_to_start(self):
self.go_to_pose(self.start_eef_pos, self.start_eef_quat)
def set_neutral(self):
"""
Sets both arms back into a neutral pose.
"""
print("Moving to neutral pose...")
self._right_arm.move_to_neutral(speed = 0.15)
def clean_shutdown(self):
print("\nExiting example...")
# return to normal
self._reset_control_modes()
self.set_neutral()
return True
def get_control(self, action, safety_max = 0.1):
current_joint_angles = [self._right_arm.joint_angles()['right_j{}'.format(i)] for i in range(7)]
z_vel = self.get_right_hand_pos()[2] - self.start_eef_pos[2]
action = np.concatenate([action, [2 * z_vel]])
# get x,y,z velocity in base frame
action_in_base = self.base_rot_in_eef.dot(action)
action_in_base = np.clip(action_in_base,-safety_max,safety_max)
# Correct for any change in orientation using a proportional controller
current_right_hand_quat = self.get_right_hand_quat()
reference_right_hand_quat = self.start_eef_quat
orn_diff = T.quat_multiply(reference_right_hand_quat, T.quat_inverse(current_right_hand_quat))
orn_diff_mat = T.quat2mat(orn_diff)
orn_diff_twice = orn_diff_mat.dot(orn_diff_mat)
# Construct pose matrix
pose_matrix = np.zeros((4, 4))
pose_matrix[:3, :3] = orn_diff_twice
pose_matrix[:3, 3] = action_in_base
pose_matrix[3, 3] = 1
# pose_matrix = T.pose2mat((action_in_base, orn_diff))
joint_velocities = self.controller.compute_joint_velocities_for_endpoint_velocity(pose_matrix,
current_joint_angles)
j_v = {}
for i in range(7):
key = 'right_j{}'.format(i)
j_v[key] = joint_velocities[i]
return j_v
def characterisation_observation_callback(self, data):
self.characterisation_observation = data
def observation_callback(self, data):
self.observation = data.data
def get_observations(self):
return copy.deepcopy(self.observation), copy.deepcopy(self.characterisation_observation)
def reset(self):
rospy.loginfo('Resetting push robot observation publisher')
self.observation = None
self.characterisation_observation = None
class Control(object):
def __init__(self):
# Make sure there is a clean shutdown
rospy.on_shutdown(self.clean_shutdown)
# Setting up Sawyer
self._right_arm = intera_interface.limb.Limb("right")
self._right_joint_names = self._right_arm.joint_names()
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self.set_neutral()
# end-effector initial pose
self.eef_pose = self._right_arm.endpoint_pose()
self.eef_quat = np.array([
self.eef_pose['orientation'].x,
self.eef_pose['orientation'].y,
self.eef_pose['orientation'].z,
self.eef_pose['orientation'].w,
])
self.base_rot_in_eef = T.quat2mat(self.eef_quat).T
# control parameters
self._rate = 500.0 # Hz
# Eef velocity control
self.controller = SawyerEEFVelocityController()
#Policy parameters
self.policy_type = 'joint'
self.line_policy = np.array([0., 0., 0.])
self.period_factor = 0.15
self.amplitude_factor = 0.1
def set_policy_type(self, policy_type):
assert policy_type == 'joint' or policy_type == 'eef_circle' or policy_type == "eef_line"
self.policy_type = policy_type
def set_period_amplitude(self, period, amplitude):
if (period is not None):
self.period_factor = period
if (amplitude is not None):
self.amplitude_factor = amplitude
def set_line_policy(self, line_policy):
if (line_policy is not None):
self.line_policy = line_policy
def cos_wave(self, elapsed):
w = self.period_factor * elapsed
return self.amplitude_factor * math.cos(w * 2 * math.pi)
def sin_wave(self, elapsed):
w = self.period_factor * elapsed
return self.amplitude_factor * math.sin(w * 2 * math.pi)
def get_policy(self, time, theshold_time=-1.):
if (self.policy_type == 'joint'):
return np.array([self.sin_wave(time) for _ in range(7)])
elif (self.policy_type == 'eef_circle'):
return np.array([self.sin_wave(time), self.cos_wave(time), 0.0])
elif (self.policy_type == 'eef_line'):
if (time < theshold_time):
return self.line_policy
else:
return np.array([0., 0., 0.])
def _reset_control_modes(self):
rate = rospy.Rate(self._rate)
for _ in xrange(100):
if rospy.is_shutdown():
return False
self._right_arm.exit_control_mode()
rate.sleep()
return True
def get_right_hand_quat(self):
eef_pose = self._right_arm.endpoint_pose()
return np.array([
eef_pose['orientation'].x,
eef_pose['orientation'].y,
eef_pose['orientation'].z,
eef_pose['orientation'].w,
])
def set_neutral(self):
"""
Sets both arms back into a neutral pose.
"""
print("Moving to neutral pose...")
self._right_arm.move_to_neutral(speed=0.1)
def clean_shutdown(self):
print("\nExiting example...")
# return to normal
self._reset_control_modes()
self.set_neutral()
return True
def get_control(self, action, safety_max=0.1):
current_joint_angles = [
self._right_arm.joint_angles()['right_j{}'.format(i)]
for i in range(7)
]
# get x,y,z velocity in base frame
action_in_base = self.base_rot_in_eef.dot(action)
action_in_base = np.clip(action_in_base, -safety_max, safety_max)
# Correct for any change in orientation using a proportional controller
current_right_hand_quat = self.get_right_hand_quat()
reference_right_hand_quat = self.eef_quat
orn_diff = T.quat_multiply(reference_right_hand_quat,
T.quat_inverse(current_right_hand_quat))
orn_diff_mat = T.quat2mat(orn_diff)
orn_diff_twice = orn_diff_mat.dot(orn_diff_mat)
# Construct pose matrix
pose_matrix = np.zeros((4, 4))
pose_matrix[:3, :3] = orn_diff_twice
pose_matrix[:3, 3] = action_in_base
pose_matrix[3, 3] = 1
# pose_matrix = T.pose2mat((action_in_base, orn_diff))
joint_velocities = self.controller.compute_joint_velocities_for_endpoint_velocity(
pose_matrix, current_joint_angles)
j_v = {}
for i in range(7):
key = 'right_j{}'.format(i)
j_v[key] = joint_velocities[i]
return j_v
def callback(self, data):
self.control = data.data
rospy.loginfo(type(self.control))
rospy.loginfo(self.control)