def main():
rospy.init_node("ellipsoidal_controller_backend")
cart_arm = create_ep_arm('l', PR2ArmJTransposeTask,
controller_name='%s_cart_jt_task',
end_link="%s_gripper_shaver45_frame")
ell_backend = EllipsoidalInterfaceBackend(cart_arm)
ell_backend.disable_interface("Setting up arm.")
if True:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
rospy.sleep(1)
joint_arm = create_ep_arm('l', PR2ArmJointTraj)
setup_angles = [0, 0, np.pi/2, -np.pi/2, -np.pi, -np.pi/2, -np.pi/2]
joint_arm.set_ep(setup_angles, 5)
rospy.sleep(5)
ctrl_switcher.carefree_switch('l', '%s_cart_jt_task',
"$(find hrl_rfh_fall_2011)/params/l_jt_task_shaver45.yaml")
rospy.sleep(1)
if True:
rospy.sleep(1)
setup_angles = [0, 0, np.pi/2, -np.pi/2, -np.pi, -np.pi/2, -np.pi/2]
cart_arm.set_posture(setup_angles)
cart_arm.set_gains([200, 800, 800, 80, 80, 80], [15, 15, 15, 1.2, 1.2, 1.2])
ell_backend.controller.reset_arm_orientation(8)
ell_backend.enable_interface("Controller ready.")
rospy.spin()
ell_backend.print_statistics()
def main():
rospy.init_node("test_ellipsoid_controller")
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
rospy.sleep(1)
joint_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
setup_angles = [0, 0, np.pi/2, -np.pi/2, -np.pi, -np.pi/2, -np.pi/2]
#joint_controller = EPArmController(joint_arm, 0.1, "joint_ep_controller")
#joint_controller.execute_interpolated_ep(setup_angles, 10)
ctrl_switcher.carefree_switch('l', '%s_cart', None)
rospy.sleep(1)
cart_arm = create_pr2_arm('l', PR2ArmJTransposeTask, end_link="%s_gripper_shaver45_frame")
cart_arm.set_posture(setup_angles)
#cart_arm.set_posture(cart_arm.get_joint_angles(wrapped=False))
cart_arm.set_gains([110, 600, 600, 40, 40, 40], [15, 15, 15, 1.2, 1.2, 1.2])
ell_controller = EllipsoidController(cart_arm)
ell_controller.reset_arm_orientation(8)
pg.init()
screen = pg.display.set_mode((300,300))
while not rospy.is_shutdown():
pg.event.get()
keys = pg.key.get_pressed()
dur = 5
if True:
r = np.random.randint(6)
if r == 0:
ell_controller.command_move([ 1, 0, 0], dur)
elif r == 1:
ell_controller.command_move([-1, 0, 0], dur)
elif r == 2:
ell_controller.command_move([ 0, 1, 0], dur)
elif r == 3:
ell_controller.command_move([ 0, -1, 0], dur)
elif r == 4:
ell_controller.command_move([ 0, 0, 1], dur)
elif r == 5:
ell_controller.command_move([ 0, 0, -1], dur)
rospy.sleep(1)
else:
if keys[pg.K_w]:
ell_controller.command_move([-1, 0, 0], dur)
if keys[pg.K_s]:
ell_controller.command_move([ 1, 0, 0], dur)
if keys[pg.K_a]:
ell_controller.command_move([ 0, 1, 0], dur)
if keys[pg.K_d]:
ell_controller.command_move([ 0, -1, 0], dur)
if keys[pg.K_q]:
ell_controller.command_move([ 0, 0, 1], dur)
if keys[pg.K_e]:
ell_controller.command_move([ 0, 0, -1], dur)
rospy.sleep(0.05)
def main():
rospy.init_node("carefree_switch_controller", sys.argv)
if len(sys.argv) <= 1 or sys.argv[1] in ["-h", "--help"]:
print "Usage: arm_side new_controller '[param_file]'"
return
cs = ControllerSwitcher()
if len(sys.argv) >= 4:
param_path = sys.argv[3]
if 'find' not in param_path:
param_path = "$(find hrl_pr2_arms)/params/" + param_path
print cs.carefree_switch(sys.argv[1], sys.argv[2], param_path)
else:
print cs.carefree_switch(sys.argv[1], sys.argv[2])
class SetupTaskController(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded'])
self.ctrl_switcher = ControllerSwitcher()
self.arm = create_pr2_arm('l', PR2ArmJTransposeTask, end_link="%s_gripper_shaver45_frame")
def execute(self, userdata):
self.ctrl_switcher.carefree_switch('l', '%s_cart_jt_task',
"$(find hrl_rfh_fall_2011)/params/l_jt_task_shaver45.yaml")
rospy.sleep(1)
setup_angles = [0, 0, np.pi/2, -np.pi/2, -np.pi, -np.pi/2, -np.pi/2]
self.arm.set_posture(setup_angles)
self.arm.set_gains([200, 800, 800, 80, 80, 80], [15, 15, 15, 1.2, 1.2, 1.2])
return 'succeeded'
def setup_servoing_arms(msg):
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('r', joint_ctrl, reset=False)
ctrl_switcher.carefree_switch('l', joint_ctrl, reset=False)
r_arm = create_pr2_arm('r', PR2ArmJointTrajectory, controller_name=joint_ctrl)
l_arm = create_pr2_arm('l', PR2ArmJointTrajectory, controller_name=joint_ctrl)
r_ep_arm_ctrl = EPArmController(r_arm)
l_ep_arm_ctrl = EPArmController(l_arm)
r_ep_arm_ctrl.execute_interpolated_ep([-1.91, 1.25, -1.93, -1.53, 0.33, -0.03, 0.0],
15, blocking=False)
l_ep_arm_ctrl.execute_interpolated_ep([1.91, 1.25, 1.93, -1.53, -0.33, -0.03, -3.09],
15, blocking=True)
return EmptyResponse()
def main():
rospy.init_node("carefree_switch_controller", sys.argv)
if len(sys.argv) <= 1:
print "Usage: arm new_controller [param_file]"
return
cs = ControllerSwitcher()
if len(sys.argv) >= 4:
if len(sys.argv) >= 5:
pkg = sys.argv[3]
param_file = sys.argv[4]
else:
pkg = 'hrl_pr2_arms'
param_file = sys.argv[3]
param_path = "$(find %s)/params/%s" % (pkg, param_file)
print cs.carefree_switch(sys.argv[1], sys.argv[2], param_path)
else:
print cs.carefree_switch(sys.argv[1], sys.argv[2])
def main():
rospy.init_node("carefree_switch_controller", sys.argv)
if len(sys.argv) <= 1:
print "Usage: arm new_controller [param_file]"
return
cs = ControllerSwitcher()
if len(sys.argv) >= 4:
if len(sys.argv) >= 5:
pkg = sys.argv[3]
param_file = sys.argv[4]
else:
pkg = 'hrl_pr2_arms'
param_file = sys.argv[3]
param_path = "$(find %s)/params/%s" % (pkg, param_file)
print cs.carefree_switch(sys.argv[1], sys.argv[2], param_path)
else:
print cs.carefree_switch(sys.argv[1], sys.argv[2])
def main():
rospy.init_node("test_ellipsoid_controller")
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
ctrl_switcher.carefree_switch('r', '%s_arm_controller', None)
rospy.sleep(1)
l_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
r_arm = create_pr2_arm('r', PR2ArmJointTrajectory)
l_tuck_angles = [ 0.14228106, 1.29643293, 1.78480255, -1.56470338, 1.16505304,
-0.09788312, 0.23542476]
r_tuck_angles = [ 0.01289596, 1.02437885, -1.34551339, -1.78272859, 0.38331793,
-1.28334274, 0.02605728]
l_joint_controller = EPArmController(l_arm, 0.1, "l_joint_ep_controller")
r_joint_controller = EPArmController(r_arm, 0.1, "r_joint_ep_controller")
l_joint_controller.execute_interpolated_ep(l_tuck_angles, 10)
r_joint_controller.execute_interpolated_ep(r_tuck_angles, 10)
class SetupTaskController(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded'])
self.ctrl_switcher = ControllerSwitcher()
self.arm = create_pr2_arm('l',
PR2ArmJTransposeTask,
end_link="%s_gripper_shaver45_frame")
def execute(self, userdata):
self.ctrl_switcher.carefree_switch(
'l', '%s_cart_jt_task',
"$(find hrl_rfh_fall_2011)/params/l_jt_task_shaver45.yaml")
rospy.sleep(1)
setup_angles = [
0, 0, np.pi / 2, -np.pi / 2, -np.pi, -np.pi / 2, -np.pi / 2
]
self.arm.set_posture(setup_angles)
self.arm.set_gains([200, 800, 800, 80, 80, 80],
[15, 15, 15, 1.2, 1.2, 1.2])
return 'succeeded'
def move(self):
# this is for robot_mechanism_controllers/CartesianTwistController
ctrl_switcher = ControllerSwitcher()
initialPose(ctrl_switcher)
rospy.loginfo('create the controller')
ctrl_switcher.carefree_switch('r', '%s_cart', '$(find visual_servo)/params/rmc_cartTwist_params.yaml')
rospy.sleep(0.5)
pub = rospy.Publisher('r_cart/command', Twist)
rospy.loginfo('created the publisher obj')
pose = Twist()
pose.linear.x = 0.00
pose.linear.y = 0.00
pose.linear.z = -.05
pose.angular.x = 0.1
pose.angular.y = 0.1
pose.angular.z = 0.1
while not rospy.is_shutdown():
rospy.loginfo('Publishing following message: %s'%pose)
pub.publish(pose)
rospy.sleep(2.0)
def main():
rospy.init_node("arm_cart_control_backend")
ctrl_switcher = ControllerSwitcher()
if False:
ctrl_switcher.carefree_switch(
'r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch(
'r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r',
PR2ArmJointTrajectory,
controller_name='%s_joint_controller_low')
q = [
-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314,
-1.82146607, -1.85187734
]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch(
'r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r',
PR2ArmCartesianPostureBase,
controller_name='%s_cart_low_rfh')
r_arm.set_posture()
rospy.sleep(0.2)
l_arm = None
#l_arm = create_pr2_arm('l', PR2ArmCartesianPostureBase)
#l_arm.set_posture()
rospy.sleep(0.2)
cart_ctrl = ArmCartCtrlBackend(r_arm, l_arm, MONITOR_RATE)
cart_ctrl.backend_loop()
def main():
rospy.init_node("arm_cart_vel_control")
if True:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch(
'r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch(
'r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r',
PR2ArmJointTrajectory,
controller_name='%s_joint_controller_low')
q = [
-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314,
-1.82146607, -1.85187734
]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch(
'r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase)
r_arm.set_posture()
rospy.sleep(0.2)
vel_ctrl = PR2ArmCartVelocityController(r_arm)
#vel_frame = tf_trans.euler_matrix(0, -np.pi/2, 0)[:3,:3]
#vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.005, is_translation=False)
vel_frame = tf_trans.euler_matrix(0, 0, np.pi / 2)[:3, :3]
vel_ctrl.move_velocity(velocity_rot=vel_frame,
velocity=0.10,
is_translation=False)
def main():
rospy.init_node("arm_cart_control_backend")
ctrl_switcher = ControllerSwitcher()
if False:
ctrl_switcher.carefree_switch('r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch('r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r', PR2ArmJointTrajectory, controller_name='%s_joint_controller_low')
q = [-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314, -1.82146607, -1.85187734]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch('r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase, controller_name='%s_cart_low_rfh')
r_arm.set_posture()
rospy.sleep(0.2)
l_arm = None
#l_arm = create_pr2_arm('l', PR2ArmCartesianPostureBase)
#l_arm.set_posture()
rospy.sleep(0.2)
cart_ctrl = ArmCartCtrlBackend(r_arm, l_arm, MONITOR_RATE)
cart_ctrl.backend_loop()
def main():
rospy.init_node("test_ellipsoid_controller")
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
ctrl_switcher.carefree_switch('r', '%s_arm_controller', None)
rospy.sleep(1)
l_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
r_arm = create_pr2_arm('r', PR2ArmJointTrajectory)
l_tuck_angles = [
0.14228106, 1.29643293, 1.78480255, -1.56470338, 1.16505304,
-0.09788312, 0.23542476
]
r_tuck_angles = [
0.01289596, 1.02437885, -1.34551339, -1.78272859, 0.38331793,
-1.28334274, 0.02605728
]
l_joint_controller = EPArmController(l_arm, 0.1, "l_joint_ep_controller")
r_joint_controller = EPArmController(r_arm, 0.1, "r_joint_ep_controller")
l_joint_controller.execute_interpolated_ep(l_tuck_angles, 10)
r_joint_controller.execute_interpolated_ep(r_tuck_angles, 10)
def main():
rospy.init_node("arm_cart_vel_control")
if True:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch('r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r', PR2ArmJointTrajectory, controller_name='%s_joint_controller_low')
q = [-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314, -1.82146607, -1.85187734]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch('r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase)
r_arm.set_posture()
rospy.sleep(0.2)
vel_ctrl = PR2ArmCartVelocityController(r_arm)
#vel_frame = tf_trans.euler_matrix(0, -np.pi/2, 0)[:3,:3]
#vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.005, is_translation=False)
vel_frame = tf_trans.euler_matrix(0, 0, np.pi/2)[:3,:3]
vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.10, is_translation=False)
class Shaving_manager():
def __init__(self):
self.tf = TransformListener()
self.jtarms = jttask_utils.jt_task_utils(self.tf)
self.controller_switcher = ControllerSwitcher()
##### Subscribers ####
self.disc_sub = rospy.Subscriber('wt_disc_shaving_step', Int8, self.disc_change_pose)
self.disc_sub.impl.add_callback(self.cancel_goals, None)
self.cont_sub = rospy.Subscriber('wt_cont_shaving_step', Point, self.cont_change_pose)
self.cont_sub.impl.add_callback(self.cancel_goals, None)
self.loc_sub = rospy.Subscriber('wt_shave_location', Int8, self.set_shave_pose)
self.loc_sub.impl.add_callback(self.cancel_goals, None)
rospy.Subscriber('wt_stop_shaving', Bool, self.stop_shaving)
#### Services ####
rospy.loginfo("Waiting for get_head_pose service")
try:
rospy.wait_for_service('/get_head_pose', 5.0)
self.get_pose_proxy = rospy.ServiceProxy('/get_head_pose', GetHeadPose)
rospy.loginfo("Found get_head_pose service")
except:
rospy.logwarn("get_head_pose service not available")
rospy.loginfo("Waiting for ellipsoid_command service")
try:
rospy.wait_for_service('/ellipsoid_command', 5.0)
self.ellipsoid_command_proxy = rospy.ServiceProxy('/ellipsoid_command', EllipsoidCommand)
rospy.loginfo("Found ellipsoid_command service")
except:
rospy.logwarn("ellipsoid_command service not available")
#### Publishers ####
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_out = rospy.Publisher('test_pose_1', PoseStamped)
self.rezero_wrench = rospy.Publisher('/netft_gravity_zeroing/rezero_wrench', Bool)
#### Data ####
self.hand_offset = 0.195
self.angle_tool_offset = 0.03
self.inline_tool_offset = 0.085
self.selected_pose = 0
self.poses = {
0 : ["near_ear",0.],
1 : ["upper_cheek",0.],
2 : ["middle_cheek",0.],
3 : ["jaw_bone",0.],
4 : ["back_neck",0.],
5 : ["nose",0.],
6 : ["chin",0.],
7 : ["mouth_corner", 0.]
}
#self.ft_wrench = WrenchStamped()
# self.force_unsafe = False
# self.ft_activity_thresh = 3
# self.ft_safety_thresh = 12
# rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.get_ft_state)
# rospy.Subscriber('/netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.get_ft_state)
if POSTURE:
rospy.sleep(3)
print 'Sending Posture'
self.jtarms.send_posture(posture='elbowup', arm=0)
# def get_ft_state(self, ws):
# self.ft_wrench = ws
# self.ft_mag = math.sqrt(ws.wrench.force.x**2 + ws.wrench.force.y**2 + ws.wrench.force.z**2)
# if self.ft_mag > self.ft_safety_thresh:
# self.force_unsafe = True
# if self.ft_mag > self.ft_activity_thresh:
# self.ft_activity = rospy.Time.now()
def stop_shaving(self, msg):
self.cancel_goals(msg)
rospy.loginfo("Stopping Shaving Behavior")
self.wt_log_out.publish(data="Stopping Shaving Behavior")
self.controller_switcher.carefree_switch('l','%s_arm_controller')
def cancel_goals(self, msg):
self.jtarms.ft_move_client.cancel_all_goals() #Stop what we're doing, as we're about to do something different, and stopping soon may be desired
self.jtarms.ft_hold_client.cancel_all_goals()
rospy.loginfo("Cancelling Active Shaving Goals")
self.wt_log_out.publish(data="Cancelling Active Shaving Goals")
def cont_change_pose(self, step):
self.controller_switcher.carefree_switch('l','%s_cart')
self.new_pose = True
req = EllipsoidCommandRequest()
req.change_latitude = int(-step.y)
req.change_longitude = int(-step.x)
req.change_height = int(step.z)
print req
print type(req.change_latitude)
self.ellipsoid_command_proxy.call(req)
rospy.loginfo("Moving Across Ellipsoid")
self.wt_log_out.publish("Moving Across Ellipsoid")
def disc_change_pose(self, step):
self.new_pose = True
self.selected_pose += step.data
if self.selected_pose > 7:
self.selected_pose = 7
self.wt_log_out.publish(data="Final position in sequence, there is no next position")
return
elif self.selected_pose < 0:
self.selected_pose = 0
self.wt_log_out.publish(data="First position in sequence, there is no previous position")
return
rospy.loginfo("Moving to "+self.poses[self.selected_pose][0])
self.wt_log_out.publish(data="Moving to "+self.poses[self.selected_pose][0])
ellipse_pose = self.get_pose_proxy(self.poses[self.selected_pose][0], self.poses[self.selected_pose][1]).pose
self.adjust_pose(ellipse_pose)
def set_shave_pose(self, num):
self.selected_pose = num.data
rospy.loginfo("Moving to "+self.poses[self.selected_pose][0])
self.wt_log_out.publish(data="Moving to "+self.poses[self.selected_pose][0])
ellipse_pose = self.get_pose_proxy(self.poses[self.selected_pose][0], self.poses[self.selected_pose][1]).pose
self.adjust_pose(ellipse_pose)
def adjust_pose(self, ellipse_pose):
self.controller_switcher.carefree_switch('l','%s_cart')
self.jtarms.ft_move_client.cancel_all_goals() #Stop what we're doing, as we're about to do something different, and stopping soon may be desired
self.jtarms.ft_hold_client.cancel_all_goals()
#self.test_out.publish(ellipse_pose)
print "New Position Received, should stop current action"
self.tf.waitForTransform(ellipse_pose.header.frame_id, 'torso_lift_link', ellipse_pose.header.stamp, rospy.Duration(3.0))
torso_pose = self.tf.transformPose('torso_lift_link', ellipse_pose)
if TOOL == 'inline':
goal_pose = self.jtarms.pose_frame_move(torso_pose, -(self.hand_offset + self.inline_tool_offset), arm=0)
approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0)
elif TOOL == '90':
goal_pose = self.jtarms.pose_frame_move(torso_pose, 0.015)
elif TOOL == '45':
goal_pose = torso_pose
approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0)
traj_to_approach = self.jtarms.build_trajectory(approach_pose, arm=0)
traj_appr_to_goal = self.jtarms.build_trajectory(goal_pose, approach_pose, arm=0)
self.ft_move(traj_to_approach)
self.rezero_wrench.publish(data=True)
self.ft_move(traj_appr_to_goal, ignore_ft=False)
retreat_traj = self.jtarms.build_trajectory(approach_pose, arm=0)
escape_traj = self.jtarms.build_trajectory(approach_pose, arm=0, steps=30)
self.jtarms.ft_hold_client.send_goal(FtHoldGoal(2.,8,10,rospy.Duration(10)))#activity_thresh, z_unsafe, mag_unsafe, timeout
print "Waiting for hold result"
finished = self.jtarms.ft_hold_client.wait_for_result(rospy.Duration(0))
print "Finished before Timeout: %s" %finished
print "Done Waiting"
hold_result = self.jtarms.ft_hold_client.get_result()
print "Hold Result:"
print hold_result
if hold_result.unsafe:
self.ft_move(escape_traj)
elif hold_result.timeout:
self.ft_move(retreat_traj)
def ft_move(self, traj, ft_thresh=2, ignore_ft=True):
self.jtarms.ft_move_client.send_goal(FtMoveGoal(traj, ft_thresh, ignore_ft),feedback_cb=self.ft_move_feedback)
finished_within_timeout = self.jtarms.ft_move_client.wait_for_result(rospy.Duration(0.25*len(traj)))
if finished_within_timeout:
result = self.jtarms.ft_move_client.get_result()
if result.all_sent:
rospy.loginfo("Full Trajectory Completed")
self.wt_log_out.publish(data="Full Trajectory Completed")
elif result.contact:
rospy.loginfo("Stopped Trajectory upon contact")
self.wt_log_out.publish(data="Stopped Trajectory upon contact")
else:
self.jtarms.ft_move_client.cancel_goal()
rospy.loginfo("Failed to complete movement within timeout period.")
self.wt_log_out.publish(data="Failed to complete movement within timeout period.")
def ft_move_feedback(self, fdbk):
pct = 100.*float(fdbk.current)/float(fdbk.total)
#rospy.loginfo("Movement is %2.0f percent complete." %pct)
self.wt_log_out.publish(data="Movement to %s is %2.0f percent complete."
%(self.poses[self.selected_pose][0], pct))
#def hold_ft_aware(self, escape_pose, arm=0):
# self.jtarms.force_stopped = False
# self.force_unsafe = False
# escape_traj = self.jtarms.build_trajectory(escape_pose)
# time_since_activity = rospy.Duration(0)
# self.ft_activity = rospy.Time.now()
# while not (rospy.is_shutdown() or self.force_unsafe or time_since_activity>rospy.Duration(10) or self.new_pose):
# time_since_activity = rospy.Time.now()-self.ft_activity
# rospy.sleep(0.01)
# if self.force_unsafe:
# rospy.loginfo("Unsafe High Forces, moving away!")
# self.wt_log_out.publish(data="Unsafe High Forces, moving away!")
# self.jtarms.goal_pub[arm].publish(escape_pose)
# return
# if time_since_activity>rospy.Duration(10):
# rospy.loginfo("10s of inactivity, moving away")
# self.wt_log_out.publish(data="10s of inactivity, moving away")
# if self.new_pose:
# rospy.loginfo("New Pose Received")
# self.wt_log_out.publish(data="New Pose Received")
# self.new_pose = False
# self.jtarms.send_traj(escape_traj)
def test(self):
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'torso_lift_link'
goal_pose.header.stamp = rospy.Time.now()
goal_pose.pose.position = Point(0.8, 0.3, 0.0)
goal_pose.pose.orientation = Quaternion(1,0,0,0)
approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0)
traj_to_approach = self.jtarms.build_trajectory(approach_pose, arm=0)
traj_appr_to_goal = self.jtarms.build_trajectory(goal_pose, approach_pose, arm=0)
raw_input("Move to approach pose")
self.jtarms.send_traj(traj_to_approach)
self.jtarms.force_stopped = False
self.force_unsafe = False
raw_input("Move to contact pose")
self.jtarms.force_stopped = False
self.force_unsafe = False
self.jtarms.send_traj_to_contact(traj_appr_to_goal)
raw_input("Hold under force constraints")
self.jtarms.force_stopped = False
self.force_unsafe = False
self.hold_ft_aware(approach_pose, arm=0)
rospy.loginfo("Finished Testing")
class TrajPlayback(object):
##
# @param arm_char 'r' or 'l' to choose which arm to load
# @param ctrl_name name of the controller to load from the param_file
# @param param_file Location of the parameter file which specifies the controller
# (e.g. "$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml")
def __init__(self, arm_char, ctrl_name="%s_arm_controller", param_file=None):
self.arm_char = arm_char
self.ctrl_name = ctrl_name
self.param_file = param_file
self.cur_joint_traj = None
self.running = False
self.is_paused = False
self.ctrl_switcher = ControllerSwitcher()
#rospy.loginfo('[trajectory_playback] Started Traj Playback for %s arm with %s controller.' %(self.arm_char, self.ctrl_name))
##
# Switches controllers and returns an instance of the arm to control
def load_arm(self):
self.ctrl_switcher.carefree_switch(self.arm_char, self.ctrl_name,
self.param_file, reset=False)
return create_ep_arm(self.arm_char, PR2ArmJointTraj,
controller_name=self.ctrl_name, timeout=8)
##
# Plays back the specified trajectory. The arm must currently be at the first
# joint configuration specified in the joint trajectory within a certain degree of
# tolerance.
# @param joint_trajectory List of lists of length 7 representing joint angles to move through.
# @param rate Frequency with which to iterate through the list.
# @param blocking If True, the function will wait until done, if False, it will return
# immediately
def execute(self, joint_trajectory, rate, blocking=True):
if len(joint_trajectory) == 0:
return True
if self.running:
rospy.logerr("[arm_pose_move_controller] Trajectory already in motion.")
return False
self.cur_arm = self.load_arm()
if not self.can_exec_traj(joint_trajectory):
rospy.logwarn("[arm_pose_move_controller] Arm not at trajectory start.")
return True
def exec_traj(te):
self.cur_idx = 0
self.stop_traj = False
self.is_paused = False
self.running = True
rospy.loginfo("[arm_pose_move_controller] Starting trajectory.")
r = rospy.Rate(rate)
global stop
while (not rospy.is_shutdown() and
not self.stop_traj and
self.cur_idx < len(joint_trajectory) and
stop != "STOP"):
self.cur_arm.set_ep(joint_trajectory[self.cur_idx], 1./rate)
self.cur_idx += 1
r.sleep()
if stop == "STOP":
self.stop_moving()
self.stop_traj = True
return
self.cur_arm.set_ep(self.cur_arm.get_ep(), 0.3)
if self.cur_idx < len(joint_trajectory):
self.cur_joint_traj = joint_trajectory[self.cur_idx:]
successful = False
else:
self.cur_joint_traj = None
self.is_paused = False
successful = True
self.last_rate = rate
self.running = False
return successful
if blocking:
return exec_traj(None)
else:
self.exec_traj_timer = rospy.Timer(rospy.Duration(0.01), exec_traj, oneshot=True)
return True
##
# Executes a linearly interpolated trajectory from the current joint angles to the
# q_goal angles.
# @param q_goal List of length 7 representing the desired end configuration.
# @param rate Rate with which to execute trajectory.
# @param velocity Speed (~rad/s) to move based on a heursitic which weighs relative
# joint speeds. The elbow will not move quicker than the velocity
# in rad/s.
# @param blocking If True, the function will wait until done, if False, it will return
# immediately
def move_to_angles(self, q_goal, rate=RATE, velocity=0.1, blocking=True):
traj = self.get_angle_traj(q_goal, rate, velocity)
if len(traj) == 0:
return True
return self.execute(traj, rate, blocking)
def get_angle_traj(self, q_goal, rate=RATE, velocity=0.1):
self.cur_arm = self.load_arm()
q_cur = self.cur_arm.get_ep()
diff = self.cur_arm.difference_joints(q_goal, q_cur)
max_ang = np.max(np.fabs(diff) * JOINT_VELOCITY_WEIGHT)
time_traj = max_ang / velocity
steps = np.round(rate * time_traj)
if steps == 0:
return []
t_vals = np.linspace(0., 1., steps)
return [q_cur + diff * t for t in t_vals]
##
# Determines whether or not the arm can execute the trajectory by checking the first
# joint configuration and seeing whether or not it is within joint tolerances.
# @param joint_trajectory List of lists of length 7 representing joint angles to move through.
# @return True if the arm is at the beginning, False otherwise.
def can_exec_traj(self, joint_trajectory):
if len(joint_trajectory) == 0:
return True
q_cur = self.cur_arm.get_ep()
q_init = joint_trajectory[0]
diff = self.cur_arm.difference_joints(q_cur, q_init)
return np.all(np.fabs(diff) < JOINT_TOLERANCES)
def is_moving(self):
return self.running
##
# Pauses the movement of the trajectory but doesn't reset its position in the array.
def pause_moving(self):
if self.is_moving():
self.stop_traj = True
while not rospy.is_shutdown() and self.running:
rospy.sleep(0.01)
self.is_paused = True
return True
return False
##
# Restarts the currently running movement after being paused.
def restart_moving(self, blocking=True):
if not self.is_paused or self.cur_joint_traj is None:
return False
self.execute(self.cur_joint_traj, self.last_rate, blocking=blocking)
return True
##
# Stops the movement of the trajectory and resets the trajectory so it cannot restart.
def stop_moving(self):
self.stop_traj = True
self.is_paused = False
self.cur_joint_traj = None
while not rospy.is_shutdown() and self.running:
rospy.sleep(0.01)
def preempt(self):
self.stop_moving()
def run(data):
action = data.data
print ("Traj playback heard: %s, starting." % action)
task_set = rospy.Publisher('task_check', String)
task_set.publish("Starting %s" %action)
#Not important, not running from command line
from optparse import OptionParser
p = OptionParser()
p.add_option('-f', '--filename', dest="filename", default="",
help="File to save trajectory to or load from.")
p.add_option('-l', '--left_arm', dest="left_arm",
action="store_true", default=False,
help="Use left arm.")
p.add_option('-r', '--right_arm', dest="right_arm",
action="store_true", default=False,
help="Use right arm.")
p.add_option('-s', '--save_mode', dest="save_mode",
action="store_true", default=False,
help="Saving mode.")
p.add_option('-t', '--traj_mode', dest="traj_mode",
action="store_true", default=False,
help="Trajectory mode.")
p.add_option('-c', '--ctrl_name', dest="ctrl_name", default=CTRL_NAME_LOW,
help="Controller to run the playback with.")
p.add_option('-p', '--params', dest="param_file", default=PARAM_FILE_LOW, #None,
help="YAML file to save parameters in or load from.")
p.add_option('-v', '--srv_mode', dest="srv_mode",
action="store_true", default=False,
help="Server mode.")
p.add_option('-y', '--playback_mode', dest="playback_mode",
action="store_true", default=False,
help="Plays back trajectory immediately.")
p.add_option('-z', '--reverse', dest="reverse",
action="store_true", default=False,
help="Plays back trajectory in reverse.")
(opts, args) = p.parse_args()
#Selects arm and action based on action taken from msg string
if opts.right_arm:
arm_char = 'r'
elif opts.left_arm:
arm_char = 'l'
else:
print "Traj playback selecting default left arm."
arm_char = 'l'
if action == "GoToHome":
opts.traj_mode = False
opts.playback_mode = True
filename = 'GoToHome'
elif action == "HomeToBowl":
opts.traj_mode = False
opts.playback_mode = True
filename = 'HomeToBowl'
elif action == "ScoopYogurt":
opts.traj_mode = False
opts.playback_mode = True
filename = 'ScoopYogurt'
elif action == "MoveToCheck":
opts.traj_mode = False
opts.playback_mode = True
filename = 'MoveToCheck'
elif action == "SwitchControllers":
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch(arm_char, CTRL_NAME_DEFAULT, None, reset=False)
print "Controller returned to default"
opts.traj_mode = False
opts.playback_mode = False
task_set = rospy.Publisher('task_check', String)
task_return = action + "Done"
task_set.publish(task_return)
running = False
return
elif opts.filename == "":
print ("Heard: %s, will keep listening." % action)
running = False
return
opts.filename = filename
#Traj playback action is called here. For Yogurt delivery only playback_mode is needed
if opts.save_mode:
assert(opts.right_arm + opts.left_arm == 1)
if opts.traj_mode:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch(arm_char, CTRL_NAME_NONE, PARAM_FILE_NONE, reset=False)
traj_saver = TrajectorySaver(RATE, CTRL_NAME_NONE)
raw_input("Press enter to start recording")
traj_saver.record_trajectory(arm_char, blocking=False)
import curses
def wait_for_key(stdscr):
curses.init_pair(1, curses.COLOR_RED, curses.COLOR_WHITE)
stdscr.addstr(0,0, "Press any key to stop!", curses.color_pair(1) )
stdscr.refresh()
c = 0
while not rospy.is_shutdown() and c == 0:
c = stdscr.getch()
curses.wrapper(wait_for_key)
traj_saver.stop_record(roslaunch.substitution_args.resolve_args(opts.filename))
ctrl_switcher.carefree_switch(arm_char, opts.ctrl_name, PARAM_FILE_LOW, reset=False)
return
else:
print "FIX"
return
elif opts.srv_mode:
traj_srv = TrajectoryServer(arm_char, "/trajectory_playback_" + arm_char,
opts.ctrl_name, opts.param_file)
rospy.spin()
return
#Should only run in playback_mode
elif opts.playback_mode:
print ("starting to perform traj action for: " + action)
if opts.traj_mode:
exec_traj_from_file(opts.filename,
ctrl_name=opts.ctrl_name,
param_file=opts.param_file,
reverse=opts.reverse,
blocking=True)
#Should not run in traj_mode
else:
print "Beginning moving to trajectory start"
move_to_setup_from_file(opts.filename,
ctrl_name=opts.ctrl_name,
param_file=opts.param_file,
reverse=opts.reverse,
blocking=True)
print "Finished moving to trajectory start"
print "Beginning trajectory"
exec_traj_from_file(opts.filename,
ctrl_name=opts.ctrl_name,
param_file=opts.param_file,
reverse=opts.reverse,
blocking=True)
print "Done with playback"
task_set = rospy.Publisher('task_check', String)
task_return = action + "Done"
task_set.publish(task_return)
print "finished trajectory: ", task_return
action = ""
return
class Shaving_manager():
def __init__(self):
self.tf = TransformListener()
self.jtarms = jttask_utils.jt_task_utils(self.tf)
self.controller_switcher = ControllerSwitcher()
##### Subscribers ####
self.disc_sub = rospy.Subscriber('wt_disc_shaving_step', Int8,
self.disc_change_pose)
self.disc_sub.impl.add_callback(self.cancel_goals, None)
self.cont_sub = rospy.Subscriber('wt_cont_shaving_step', Point,
self.cont_change_pose)
self.cont_sub.impl.add_callback(self.cancel_goals, None)
self.loc_sub = rospy.Subscriber('wt_shave_location', Int8,
self.set_shave_pose)
self.loc_sub.impl.add_callback(self.cancel_goals, None)
rospy.Subscriber('wt_stop_shaving', Bool, self.stop_shaving)
#### Services ####
rospy.loginfo("Waiting for get_head_pose service")
try:
rospy.wait_for_service('/get_head_pose', 5.0)
self.get_pose_proxy = rospy.ServiceProxy('/get_head_pose',
GetHeadPose)
rospy.loginfo("Found get_head_pose service")
except:
rospy.logwarn("get_head_pose service not available")
rospy.loginfo("Waiting for ellipsoid_command service")
try:
rospy.wait_for_service('/ellipsoid_command', 5.0)
self.ellipsoid_command_proxy = rospy.ServiceProxy(
'/ellipsoid_command', EllipsoidCommand)
rospy.loginfo("Found ellipsoid_command service")
except:
rospy.logwarn("ellipsoid_command service not available")
#### Publishers ####
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_out = rospy.Publisher('test_pose_1', PoseStamped)
self.rezero_wrench = rospy.Publisher(
'/netft_gravity_zeroing/rezero_wrench', Bool)
#### Data ####
self.hand_offset = 0.195
self.angle_tool_offset = 0.03
self.inline_tool_offset = 0.085
self.selected_pose = 0
self.poses = {
0: ["near_ear", 0.],
1: ["upper_cheek", 0.],
2: ["middle_cheek", 0.],
3: ["jaw_bone", 0.],
4: ["back_neck", 0.],
5: ["nose", 0.],
6: ["chin", 0.],
7: ["mouth_corner", 0.]
}
#self.ft_wrench = WrenchStamped()
# self.force_unsafe = False
# self.ft_activity_thresh = 3
# self.ft_safety_thresh = 12
# rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.get_ft_state)
# rospy.Subscriber('/netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.get_ft_state)
if POSTURE:
rospy.sleep(3)
print 'Sending Posture'
self.jtarms.send_posture(posture='elbowup', arm=0)
# def get_ft_state(self, ws):
# self.ft_wrench = ws
# self.ft_mag = math.sqrt(ws.wrench.force.x**2 + ws.wrench.force.y**2 + ws.wrench.force.z**2)
# if self.ft_mag > self.ft_safety_thresh:
# self.force_unsafe = True
# if self.ft_mag > self.ft_activity_thresh:
# self.ft_activity = rospy.Time.now()
def stop_shaving(self, msg):
self.cancel_goals(msg)
rospy.loginfo("Stopping Shaving Behavior")
self.wt_log_out.publish(data="Stopping Shaving Behavior")
self.controller_switcher.carefree_switch('l', '%s_arm_controller')
def cancel_goals(self, msg):
self.jtarms.ft_move_client.cancel_all_goals(
) #Stop what we're doing, as we're about to do something different, and stopping soon may be desired
self.jtarms.ft_hold_client.cancel_all_goals()
rospy.loginfo("Cancelling Active Shaving Goals")
self.wt_log_out.publish(data="Cancelling Active Shaving Goals")
def cont_change_pose(self, step):
self.controller_switcher.carefree_switch('l', '%s_cart')
self.new_pose = True
req = EllipsoidCommandRequest()
req.change_latitude = int(-step.y)
req.change_longitude = int(-step.x)
req.change_height = int(step.z)
print req
print type(req.change_latitude)
self.ellipsoid_command_proxy.call(req)
rospy.loginfo("Moving Across Ellipsoid")
self.wt_log_out.publish("Moving Across Ellipsoid")
def disc_change_pose(self, step):
self.new_pose = True
self.selected_pose += step.data
if self.selected_pose > 7:
self.selected_pose = 7
self.wt_log_out.publish(
data="Final position in sequence, there is no next position")
return
elif self.selected_pose < 0:
self.selected_pose = 0
self.wt_log_out.publish(
data="First position in sequence, there is no previous position"
)
return
rospy.loginfo("Moving to " + self.poses[self.selected_pose][0])
self.wt_log_out.publish(data="Moving to " +
self.poses[self.selected_pose][0])
ellipse_pose = self.get_pose_proxy(
self.poses[self.selected_pose][0],
self.poses[self.selected_pose][1]).pose
self.adjust_pose(ellipse_pose)
def set_shave_pose(self, num):
self.selected_pose = num.data
rospy.loginfo("Moving to " + self.poses[self.selected_pose][0])
self.wt_log_out.publish(data="Moving to " +
self.poses[self.selected_pose][0])
ellipse_pose = self.get_pose_proxy(
self.poses[self.selected_pose][0],
self.poses[self.selected_pose][1]).pose
self.adjust_pose(ellipse_pose)
def adjust_pose(self, ellipse_pose):
self.controller_switcher.carefree_switch('l', '%s_cart')
self.jtarms.ft_move_client.cancel_all_goals(
) #Stop what we're doing, as we're about to do something different, and stopping soon may be desired
self.jtarms.ft_hold_client.cancel_all_goals()
#self.test_out.publish(ellipse_pose)
print "New Position Received, should stop current action"
self.tf.waitForTransform(ellipse_pose.header.frame_id,
'torso_lift_link', ellipse_pose.header.stamp,
rospy.Duration(3.0))
torso_pose = self.tf.transformPose('torso_lift_link', ellipse_pose)
if TOOL == 'inline':
goal_pose = self.jtarms.pose_frame_move(
torso_pose,
-(self.hand_offset + self.inline_tool_offset),
arm=0)
approach_pose = self.jtarms.pose_frame_move(goal_pose,
-0.15,
arm=0)
elif TOOL == '90':
goal_pose = self.jtarms.pose_frame_move(torso_pose, 0.015)
elif TOOL == '45':
goal_pose = torso_pose
approach_pose = self.jtarms.pose_frame_move(goal_pose,
-0.15,
arm=0)
traj_to_approach = self.jtarms.build_trajectory(approach_pose, arm=0)
traj_appr_to_goal = self.jtarms.build_trajectory(goal_pose,
approach_pose,
arm=0)
self.ft_move(traj_to_approach)
self.rezero_wrench.publish(data=True)
self.ft_move(traj_appr_to_goal, ignore_ft=False)
retreat_traj = self.jtarms.build_trajectory(approach_pose, arm=0)
escape_traj = self.jtarms.build_trajectory(approach_pose,
arm=0,
steps=30)
self.jtarms.ft_hold_client.send_goal(
FtHoldGoal(2., 8, 10, rospy.Duration(
10))) #activity_thresh, z_unsafe, mag_unsafe, timeout
print "Waiting for hold result"
finished = self.jtarms.ft_hold_client.wait_for_result(
rospy.Duration(0))
print "Finished before Timeout: %s" % finished
print "Done Waiting"
hold_result = self.jtarms.ft_hold_client.get_result()
print "Hold Result:"
print hold_result
if hold_result.unsafe:
self.ft_move(escape_traj)
elif hold_result.timeout:
self.ft_move(retreat_traj)
def ft_move(self, traj, ft_thresh=2, ignore_ft=True):
self.jtarms.ft_move_client.send_goal(FtMoveGoal(
traj, ft_thresh, ignore_ft),
feedback_cb=self.ft_move_feedback)
finished_within_timeout = self.jtarms.ft_move_client.wait_for_result(
rospy.Duration(0.25 * len(traj)))
if finished_within_timeout:
result = self.jtarms.ft_move_client.get_result()
if result.all_sent:
rospy.loginfo("Full Trajectory Completed")
self.wt_log_out.publish(data="Full Trajectory Completed")
elif result.contact:
rospy.loginfo("Stopped Trajectory upon contact")
self.wt_log_out.publish(data="Stopped Trajectory upon contact")
else:
self.jtarms.ft_move_client.cancel_goal()
rospy.loginfo("Failed to complete movement within timeout period.")
self.wt_log_out.publish(
data="Failed to complete movement within timeout period.")
def ft_move_feedback(self, fdbk):
pct = 100. * float(fdbk.current) / float(fdbk.total)
#rospy.loginfo("Movement is %2.0f percent complete." %pct)
self.wt_log_out.publish(
data="Movement to %s is %2.0f percent complete." %
(self.poses[self.selected_pose][0], pct))
#def hold_ft_aware(self, escape_pose, arm=0):
# self.jtarms.force_stopped = False
# self.force_unsafe = False
# escape_traj = self.jtarms.build_trajectory(escape_pose)
# time_since_activity = rospy.Duration(0)
# self.ft_activity = rospy.Time.now()
# while not (rospy.is_shutdown() or self.force_unsafe or time_since_activity>rospy.Duration(10) or self.new_pose):
# time_since_activity = rospy.Time.now()-self.ft_activity
# rospy.sleep(0.01)
# if self.force_unsafe:
# rospy.loginfo("Unsafe High Forces, moving away!")
# self.wt_log_out.publish(data="Unsafe High Forces, moving away!")
# self.jtarms.goal_pub[arm].publish(escape_pose)
# return
# if time_since_activity>rospy.Duration(10):
# rospy.loginfo("10s of inactivity, moving away")
# self.wt_log_out.publish(data="10s of inactivity, moving away")
# if self.new_pose:
# rospy.loginfo("New Pose Received")
# self.wt_log_out.publish(data="New Pose Received")
# self.new_pose = False
# self.jtarms.send_traj(escape_traj)
def test(self):
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'torso_lift_link'
goal_pose.header.stamp = rospy.Time.now()
goal_pose.pose.position = Point(0.8, 0.3, 0.0)
goal_pose.pose.orientation = Quaternion(1, 0, 0, 0)
approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0)
traj_to_approach = self.jtarms.build_trajectory(approach_pose, arm=0)
traj_appr_to_goal = self.jtarms.build_trajectory(goal_pose,
approach_pose,
arm=0)
raw_input("Move to approach pose")
self.jtarms.send_traj(traj_to_approach)
self.jtarms.force_stopped = False
self.force_unsafe = False
raw_input("Move to contact pose")
self.jtarms.force_stopped = False
self.force_unsafe = False
self.jtarms.send_traj_to_contact(traj_appr_to_goal)
raw_input("Hold under force constraints")
self.jtarms.force_stopped = False
self.force_unsafe = False
self.hold_ft_aware(approach_pose, arm=0)
rospy.loginfo("Finished Testing")
class ExperimentSetup(object):
def __init__(self):
self.ctrl_switcher = ControllerSwitcher()
self.torso_sac = actionlib.SimpleActionClient('torso_controller/position_joint_action',
SingleJointPositionAction)
self.torso_sac.wait_for_server()
self.head_point_sac = actionlib.SimpleActionClient(
'/head_traj_controller/point_head_action',
PointHeadAction)
self.head_point_sac.wait_for_server()
rospy.loginfo("[experiment_setup] ExperimentSetup ready.")
def point_head(self):
print "Pointing head"
head_goal = PointHeadGoal()
head_goal.target = PoseConverter.to_point_stamped_msg('/torso_lift_link',
np.mat([1., 0.4, 0.]).T,
np.mat(np.eye(3)))
head_goal.target.header.stamp = rospy.Time()
head_goal.min_duration = rospy.Duration(3.)
head_goal.max_velocity = 0.2
self.head_point_sac.send_goal_and_wait(head_goal)
def adjust_torso(self):
# move torso up
tgoal = SingleJointPositionGoal()
tgoal.position = 0.238 # all the way up is 0.300
tgoal.min_duration = rospy.Duration( 2.0 )
tgoal.max_velocity = 1.0
self.torso_sac.send_goal_and_wait(tgoal)
def mirror_arm_setup(self):
self.ctrl_switcher.carefree_switch('r', 'r_joint_controller_mirror',
"$(find hrl_ellipsoidal_control)/params/mirror_params.yaml")
rospy.sleep(1)
arm = create_pr2_arm('r', PR2ArmJointTrajectory,
controller_name="r_joint_controller_mirror")
arm.set_ep([-0.26880036055585677, 0.71881299774143248,
-0.010187938126536471, -1.43747589322259,
-12.531293698878677, -0.92339874393497123,
3.3566322715405432], 5)
rospy.sleep(6)
def tool_arm_setup(self):
self.ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
rospy.sleep(1)
joint_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
setup_angles = SETUP_ANGLES
joint_arm.set_ep(setup_angles, 5)
rospy.sleep(6)
def mirror_mannequin(self):
arm = create_pr2_arm('r', PR2ArmJointTrajectory,
controller_name="r_joint_controller_mirror")
r = rospy.Rate(10)
q_act_last = arm.get_joint_angles()
while not rospy.is_shutdown():
q_act = arm.get_joint_angles()
q_ep = arm.get_ep()
new_ep = q_ep.copy()
for i in range(7):
if np.fabs(q_act[i] - q_act_last[i]) > joint_deltas[i]:
new_ep[i] = q_act[i]
arm.set_ep(new_ep, 0.1)
q_act_last = q_act
r.sleep()
def cart_controller_setup(self):
self.ctrl_switcher.carefree_switch('l', '%s_cart_jt_task',
"$(find hrl_rfh_fall_2011)/params/l_jt_task_shaver45.yaml")
self.cart_arm = create_pr2_arm('l', PR2ArmJTransposeTask,
controller_name='%s_cart_jt_task',
end_link="%s_gripper_shaver45_frame")
rospy.sleep(2)
setup_angles = SETUP_ANGLES
self.cart_arm.set_posture(setup_angles)
self.cart_arm.set_gains([200, 800, 800, 80, 80, 80], [15, 15, 15, 1.2, 1.2, 1.2])
class FaceADLsManager(object):
def __init__(self):
self.ellipsoid = EllipsoidSpace()
self.controller_switcher = ControllerSwitcher()
self.ee_frame = '/l_gripper_shaver305_frame'
self.head_pose = None
self.tfl = TransformListener()
self.is_forced_retreat = False
self.pose_traj_pub = rospy.Publisher('/haptic_mpc/goal_pose_array',
PoseArray)
self.global_input_sub = rospy.Subscriber(
"/face_adls/global_move", String, async_call(self.global_input_cb))
self.local_input_sub = rospy.Subscriber(
"/face_adls/local_move", String, async_call(self.local_input_cb))
self.clicked_input_sub = rospy.Subscriber(
"/face_adls/clicked_move", PoseStamped,
async_call(self.global_input_cb))
self.feedback_pub = rospy.Publisher('/face_adls/feedback', String)
self.global_move_poses_pub = rospy.Publisher(
'/face_adls/global_move_poses', StringArray, latch=True)
self.controller_enabled_pub = rospy.Publisher(
'/face_adls/controller_enabled', Bool, latch=True)
self.enable_controller_srv = rospy.Service(
"/face_adls/enable_controller", EnableFaceController,
self.enable_controller_cb)
self.request_registration = rospy.ServiceProxy("/request_registration",
RequestRegistration)
self.enable_mpc_srv = rospy.ServiceProxy('/haptic_mpc/enable_mpc',
EnableHapticMPC)
self.ell_params_pub = rospy.Publisher("/ellipsoid_params",
EllipsoidParams,
latch=True)
def stop_move_cb(msg):
self.stop_moving()
self.stop_move_sub = rospy.Subscriber("/face_adls/stop_move",
Bool,
stop_move_cb,
queue_size=1)
def stop_moving(self):
"""Send empty PoseArray to skin controller to stop movement"""
for x in range(2):
self.pose_traj_pub.publish(
PoseArray()) #Send empty msg to skin controller
cart_traj_msg = [
PoseConv.to_pose_msg(self.current_ee_pose(self.ee_frame))
]
head = Header()
head.frame_id = '/torso_lift_link'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
rospy.loginfo("Sent stop moving commands!")
def register_ellipse(self, mode, side):
reg_e_params = EllipsoidParams()
try:
now = rospy.Time.now()
self.tfl.waitForTransform("/base_link", "/head_frame", now,
rospy.Duration(10.))
pos, quat = self.tfl.lookupTransform("/head_frame", "/base_frame",
now)
self.head_pose = PoseStamped()
self.head_pose.header.stamp = now
self.head_pose.header.frame_id = "/base_link"
self.head_pose.pose.position = Point(*pos)
self.head_pose.pose.orientation = Quaternion(*quat)
except:
rospy.logwarn("[%s] Head registration not loaded yet." %
rospy.get_name())
return (False, reg_e_params)
reg_prefix = rospy.get_param("~registration_prefix", "")
registration_files = rospy.get_param("~registration_files", None)
if mode not in registration_files:
rospy.logerr("[%s] Mode not in registration_files parameters" %
(rospy.get_name()))
return (False, reg_e_params)
try:
bag_str = reg_prefix + registration_files[mode][side]
rospy.loginfo("[%s] Loading %s" % (rospy.get_name(), bag_str))
bag = rosbag.Bag(bag_str, 'r')
e_params = None
for topic, msg, ts in bag.read_messages():
e_params = msg
assert e_params is not None
bag.close()
except:
rospy.logerr("[%s] Cannot load registration parameters from %s" %
(rospy.get_name(), bag_str))
return (False, reg_e_params)
head_reg_mat = PoseConv.to_homo_mat(self.head_pose)
ell_reg = PoseConv.to_homo_mat(
Transform(e_params.e_frame.transform.translation,
e_params.e_frame.transform.rotation))
reg_e_params.e_frame = PoseConv.to_tf_stamped_msg(head_reg_mat**-1 *
ell_reg)
reg_e_params.e_frame.header.frame_id = self.head_reg_tf.header.frame_id
reg_e_params.e_frame.child_frame_id = '/ellipse_frame'
reg_e_params.height = e_params.height
reg_e_params.E = e_params.E
self.ell_params_pub.publish(reg_e_params)
self.ell_frame = reg_e_params.e_frame
return (True, reg_e_params)
def current_ee_pose(self, link, frame='/torso_lift_link'):
"""Get current end effector pose from tf"""
# print "Getting pose of %s in frame: %s" %(link, frame)
try:
now = rospy.Time.now()
self.tfl.waitForTransform(frame, link, now, rospy.Duration(8.0))
pos, quat = self.tfl.lookupTransform(frame, link, now)
except (LookupException, ConnectivityException, ExtrapolationException,
Exception) as e:
rospy.logwarn(
"[face_adls_manager] TF Failure getting current end-effector pose: %s"
% e)
return None
return pos, quat
def publish_feedback(self, message=None):
if message is not None:
rospy.loginfo("[face_adls_manager] %s" % message)
self.feedback_pub.publish(message)
def enable_controller_cb(self, req):
if req.enable:
face_adls_modes = rospy.get_param('/face_adls_modes', None)
params = face_adls_modes[req.mode]
self.face_side = rospy.get_param('/face_side',
'r') # TODO Make more general
self.trim_retreat = req.mode == "shaving"
self.flip_gripper = self.face_side == 'r' and req.mode == "shaving"
bounds = params['%s_bounds' % self.face_side]
self.ellipsoid.set_bounds(bounds['lat'], bounds['lon'],
bounds['height']) #TODO: Change Back
#self.ellipsoid.set_bounds((-np.pi,np.pi), (-np.pi,np.pi), (0,100))
success, e_params = self.register_ellipse(req.mode, self.face_side)
if not success:
self.publish_feedback(Messages.NO_PARAMS_LOADED)
return EnableFaceControllerResponse(False)
reg_pose = PoseConv.to_pose_stamped_msg(e_params.e_frame)
try:
now = rospy.Time.now()
reg_pose.header.stamp = now
self.tfl.waitForTransform(reg_pose.header.frame_id,
'/base_link', now,
rospy.Duration(8.0))
ellipse_frame_base = self.tfl.transformPose(
'/base_link', reg_pose)
except (LookupException, ConnectivityException,
ExtrapolationException, Exception) as e:
rospy.logwarn(
"[face_adls_manager] TF Failure converting ellipse to base frame: %s"
% e)
self.ellipsoid.load_ell_params(ellipse_frame_base, e_params.E,
e_params.is_oblate, e_params.height)
global_poses_dir = rospy.get_param("~global_poses_dir", "")
global_poses_file = params["%s_ell_poses_file" % self.face_side]
global_poses_resolved = roslaunch.substitution_args.resolve_args(
global_poses_dir + "/" + global_poses_file)
self.global_poses = rosparam.load_file(global_poses_resolved)[0][0]
self.global_move_poses_pub.publish(sorted(
self.global_poses.keys()))
#Check rotating gripper based on side of body
if self.flip_gripper:
self.gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0)
print "Rotating gripper by PI around x-axis"
else:
self.gripper_rot = trans.quaternion_from_euler(0, 0, 0)
print "NOT Rotating gripper around x-axis"
# check if arm is near head
# cart_pos, cart_quat = self.current_ee_pose(self.ee_frame)
# ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
# equals = ell_pos == self.ellipsoid.enforce_bounds(ell_pos)
# print ell_pos, equals
# if self.ellipsoid._lon_bounds[0] >= 0 and ell_pos[1] >= 0:
# arm_in_bounds = np.all(equals)
# else:
# ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi)
# min_lon = np.mod(self.ellipsoid._lon_bounds[0], 2 * np.pi)
# arm_in_bounds = (equals[0] and
# equals[2] and
# (ell_lon_1 >= min_lon or ell_lon_1 <= self.ellipsoid._lon_bounds[1]))
arm_in_bounds = True
self.setup_force_monitor()
success = True
if not arm_in_bounds:
self.publish_feedback(Messages.ARM_AWAY_FROM_HEAD)
success = False
#Switch back to l_arm_controller if necessary
if self.controller_switcher.carefree_switch('l',
'%s_arm_controller',
reset=False):
print "Controller switch to l_arm_controller succeeded"
self.publish_feedback(Messages.ENABLE_CONTROLLER)
else:
print "Controller switch to l_arm_controller failed"
success = False
self.controller_enabled_pub.publish(Bool(success))
req = EnableHapticMPCRequest()
req.new_state = 'enabled'
resp = self.enable_mpc_srv.call(req)
else:
self.stop_moving()
self.controller_enabled_pub.publish(Bool(False))
rospy.loginfo(Messages.DISABLE_CONTROLLER)
req = EnableHapticMPCRequest()
req.new_state = 'disabled'
self.enable_mpc_srv.call(req)
success = False
return EnableFaceControllerResponse(success)
def setup_force_monitor(self):
self.force_monitor = ForceCollisionMonitor()
# registering force monitor callbacks
def dangerous_cb(force):
self.stop_moving()
curr_pose = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
ell_pos, _ = self.ellipsoid.pose_to_ellipsoidal(curr_pose)
if ell_pos[2] < SAFETY_RETREAT_HEIGHT * 0.9:
self.publish_feedback(Messages.DANGEROUS_FORCE % force)
self.retreat_move(SAFETY_RETREAT_HEIGHT,
SAFETY_RETREAT_VELOCITY,
forced=True)
self.force_monitor.register_dangerous_cb(dangerous_cb)
def timeout_cb(time):
start_time = rospy.get_time()
ell_pos, _ = self.ellipsoid.pose_to_ellipsoidal(
self.current_ee_pose(self.ee_frame, '/ellipse_frame'))
rospy.loginfo(
"ELL POS TIME: %.3f " % (rospy.get_time() - start_time) +
str(ell_pos) + " times: %.3f %.3f" %
(self.force_monitor.last_activity_time, rospy.get_time()))
if ell_pos[
2] < RETREAT_HEIGHT * 0.9 and self.force_monitor.is_inactive(
):
self.publish_feedback(Messages.TIMEOUT_RETREAT % time)
self.retreat_move(RETREAT_HEIGHT, LOCAL_VELOCITY)
self.force_monitor.register_timeout_cb(timeout_cb)
def contact_cb(force):
self.force_monitor.update_activity()
if not self.is_forced_retreat:
self.stop_moving()
self.publish_feedback(Messages.CONTACT_FORCE % force)
rospy.loginfo(
"[face_adls_manZger] Arm stopped due to making contact.")
self.force_monitor.register_contact_cb(contact_cb)
self.force_monitor.start_activity()
self.force_monitor.update_activity()
self.is_forced_retreat = False
def retreat_move(self, height, velocity, forced=False):
if forced:
self.is_forced_retreat = True
cart_pos, cart_quat = self.current_ee_pose(self.ee_frame,
'/ellipse_frame')
ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal(
(cart_pos, cart_quat))
#print "Retreat EP:", ell_pos
latitude = ell_pos[0]
if self.trim_retreat:
latitude = min(latitude, TRIM_RETREAT_LATITUDE)
#rospy.loginfo("[face_adls_manager] Retreating from current location.")
retreat_pos = [latitude, ell_pos[1], height]
retreat_pos = self.ellipsoid.enforce_bounds(retreat_pos)
retreat_quat = [0, 0, 0, 1]
if forced:
cart_path = self.ellipsoid.ellipsoidal_to_pose(
(retreat_pos, retreat_quat))
cart_msg = [PoseConv.to_pose_msg(cart_path)]
else:
ell_path = self.ellipsoid.create_ellipsoidal_path(ell_pos,
ell_quat,
retreat_pos,
retreat_quat,
velocity=0.01,
min_jerk=True)
cart_path = [
self.ellipsoid.ellipsoidal_to_pose(ell_pose)
for ell_pose in ell_path
]
cart_msg = [PoseConv.to_pose_msg(pose) for pose in cart_path]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_msg)
self.pose_traj_pub.publish(pose_array)
self.is_forced_retreat = False
def global_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
rospy.loginfo("[face_adls_manager] Performing global move.")
if type(msg) == String:
self.publish_feedback(Messages.GLOBAL_START % msg.data)
goal_ell_pose = self.global_poses[msg.data]
elif type(msg) == PoseStamped:
try:
self.tfl.waitForTransform(msg.header.frame_id,
'/ellipse_frame', msg.header.stamp,
rospy.Duration(8.0))
goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg)
goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(
goal_cart_pose)
flip_quat = trans.quaternion_from_euler(-np.pi / 2, np.pi, 0)
goal_cart_quat_flipped = trans.quaternion_multiply(
goal_cart_quat, flip_quat)
goal_cart_pose = PoseConv.to_pose_stamped_msg(
'/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped))
self.publish_feedback(
'Moving around ellipse to clicked position).')
goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(
goal_cart_pose)
except (LookupException, ConnectivityException,
ExtrapolationException, Exception) as e:
rospy.logwarn(
"[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s"
% e)
return
curr_cart_pos, curr_cart_quat = self.current_ee_pose(
self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal(
(curr_cart_pos, curr_cart_quat))
retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT]
retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot,
[1., 0., 0., 0.])
approach_ell_pos = [
goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT
]
approach_ell_quat = trans.quaternion_multiply(self.gripper_rot,
goal_ell_pose[1])
final_ell_pos = [
goal_ell_pose[0][0], goal_ell_pose[0][1],
goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST
]
final_ell_quat = trans.quaternion_multiply(self.gripper_rot,
goal_ell_pose[1])
cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose(
(retreat_ell_pos, retreat_ell_quat))
cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_ret_pose)
cart_app_pose = self.ellipsoid.ellipsoidal_to_pose(
(approach_ell_pos, approach_ell_quat))
cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_app_pose)
cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose(
(final_ell_pos, final_ell_quat))
cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_goal_pose)
retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
retreat_ell_pos,
retreat_ell_quat,
velocity=0.01,
min_jerk=True)
transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(
retreat_ell_pos,
retreat_ell_quat,
approach_ell_pos,
approach_ell_quat,
velocity=0.01,
min_jerk=True)
approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(
approach_ell_pos,
approach_ell_quat,
final_ell_pos,
final_ell_quat,
velocity=0.01,
min_jerk=True)
full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj
full_cart_traj = [
self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj
]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
ps = PoseStamped()
ps.header = head
ps.pose = cart_traj_msg[0]
self.force_monitor.update_activity()
def local_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
self.stop_moving()
button_press = msg.data
curr_cart_pos, curr_cart_quat = self.current_ee_pose(
self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal(
(curr_cart_pos, curr_cart_quat))
if button_press in ell_trans_params:
self.publish_feedback(Messages.LOCAL_START %
button_names_dict[button_press])
change_trans_ep = ell_trans_params[button_press]
goal_ell_pos = [
curr_ell_pos[0] + change_trans_ep[0],
curr_ell_pos[1] + change_trans_ep[1],
curr_ell_pos[2] + change_trans_ep[2]
]
ell_path = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
goal_ell_pos,
curr_ell_quat,
velocity=ELL_LOCAL_VEL,
min_jerk=True)
elif button_press in ell_rot_params:
self.publish_feedback(Messages.LOCAL_START %
button_names_dict[button_press])
change_rot_ep = ell_rot_params[button_press]
rot_quat = trans.quaternion_from_euler(*change_rot_ep)
goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat)
ell_path = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
curr_ell_pos,
goal_ell_quat,
velocity=ELL_ROT_VEL,
min_jerk=True)
elif button_press == "reset_rotation":
self.publish_feedback(Messages.ROT_RESET_START)
ell_path = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
curr_ell_pos, (0., 0., 0., 1.),
velocity=ELL_ROT_VEL,
min_jerk=True)
else:
rospy.logerr(
"[face_adls_manager] Unknown local ellipsoidal command")
cart_traj = [
self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path
]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
self.force_monitor.update_activity()
class CartesianControllerManager(object):
def __init__(self, arm_char):
self.arm_char = arm_char
self.arm = None
self.kin = None
self.cart_ctrl = CartesianStepController()
self.ctrl_switcher = ControllerSwitcher()
self.command_move_sub = rospy.Subscriber("/face_adls/%s_cart_move" % arm_char, TwistStamped,
async_call(self.command_move_cb))
self.command_absolute_sub = rospy.Subscriber("/face_adls/%s_cart_absolute" % arm_char,
PoseStamped, async_call(self.command_absolute_cb))
self.adjust_posture_sub = rospy.Subscriber("/face_adls/%s_cart_posture" % arm_char,
Float32, self.adjust_posture_cb)
def enable_controller_cb(req):
if req.enable:
_, frame_rot = PoseConv.to_pos_rot([0]*3,
[req.frame_rot.x, req.frame_rot.y, req.frame_rot.z])
if req.velocity == 0:
req.velocity = 0.03
success = self.enable_controller(req.end_link, req.ctrl_params, req.ctrl_name,
frame_rot, velocity=req.velocity)
else:
success = self.disable_controller()
return EnableCartControllerResponse(success)
self.controller_enabled_pub = rospy.Publisher('/face_adls/%s_cart_ctrl_enabled' % arm_char,
Bool, latch=True)
self.enable_controller_srv = rospy.Service("/face_adls/%s_enable_cart_ctrl" % arm_char,
EnableCartController, enable_controller_cb)
def enable_controller(self, end_link="%s_gripper_tool_frame",
ctrl_params="$(find hrl_face_adls)/params/%s_jt_task_tool.yaml",
ctrl_name="%s_cart_jt_task_tool",
frame_rot=FLIP_PERSPECTIVE_ROT, velocity=0.03):
#frame_rot=np.mat(np.eye(3))):
rospy.loginfo("[cartesian_manager] Enabling %s controller with end link %s" %
(ctrl_name, end_link))
try:
if '%s' in end_link:
end_link = end_link % self.arm_char
if '%s' in ctrl_params:
ctrl_params = ctrl_params % self.arm_char
if '%s' in ctrl_name:
ctrl_name = ctrl_name % self.arm_char
self.ctrl_switcher.carefree_switch(self.arm_char, ctrl_name, ctrl_params, reset=False)
rospy.sleep(0.2)
cart_arm = create_ep_arm(self.arm_char, PR2ArmJTransposeTask,
controller_name=ctrl_name,
end_link=end_link, timeout=5)
self.cart_ctrl.set_arm(cart_arm)
self.arm = cart_arm
rospy.sleep(0.1)
self.cur_posture_angle = self.arm.get_joint_angles()[2]
self.adjust_posture_cb(Float32(0.))
self.frame_rot = frame_rot
self.velocity = velocity
self.controller_enabled_pub.publish(True)
except Exception as e:
print e
return False
rospy.loginfo("[cartesian_manager] Successfully enabled %s controller with end link %s" %
(ctrl_name, end_link))
return True
def disable_controller(self):
rospy.loginfo("[cartesian_manager] Disabling cartesian controller.")
self.cart_ctrl.set_arm(None)
self.arm = None
self.controller_enabled_pub.publish(False)
return True
def command_move_cb(self, msg):
if self.arm is None:
rospy.logwarn("[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names():
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = PoseConv.to_pos_rot(self.arm.get_ep())
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
_, torso_rot_ref = PoseConv.to_pos_rot(torso_B_ref)
torso_rot_ref *= self.frame_rot
ref_pos_off, ref_rot_off = PoseConv.to_pos_rot(msg)
change_pos = torso_rot_ep.T * torso_rot_ref * ref_pos_off
change_pos_xyz = change_pos.T.A[0]
ep_rot_ref = torso_rot_ep.T * torso_rot_ref
change_rot = ep_rot_ref * ref_rot_off * ep_rot_ref.T
_, change_rot_rpy = PoseConv.to_pos_euler(np.mat([0]*3).T, change_rot)
rospy.loginfo("[cartesian_manager] Command relative movement." +
str((change_pos_xyz, change_rot_rpy)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot_rpy), ((0, 0, 0), 0),
velocity=self.velocity, blocking=True)
def command_absolute_cb(self, msg):
if self.arm is None:
rospy.logwarn("[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names():
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = self.arm.get_ep()
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
ref_B_goal = PoseConv.to_homo_mat(msg)
torso_B_goal = torso_B_ref * ref_B_goal
change_pos, change_rot = PoseConv.to_pos_rot(torso_B_goal)
change_pos_xyz = change_pos.T.A[0]
rospy.loginfo("[cartesian_manager] Command absolute movement." +
str((change_pos_xyz, change_rot)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot), ((1, 1, 1), 1),
velocity=self.velocity, blocking=True)
def adjust_posture_cb(self, msg):
rospy.loginfo("[cartesian_manager] Adjusting posture")
new_posture = [None] * 7
self.cur_posture_angle += msg.data
new_posture[2] = self.cur_posture_angle
self.arm.set_posture(new_posture)
class FaceADLsManager(object):
def __init__(self):
self.ellipsoid = EllipsoidSpace()
self.controller_switcher = ControllerSwitcher()
self.ee_frame = '/l_gripper_shaver305_frame'
self.head_pose = None
self.tfl = TransformListener()
self.is_forced_retreat = False
self.pose_traj_pub = rospy.Publisher('/haptic_mpc/goal_pose_array', PoseArray)
self.global_input_sub = rospy.Subscriber("/face_adls/global_move", String,
async_call(self.global_input_cb))
self.local_input_sub = rospy.Subscriber("/face_adls/local_move", String,
async_call(self.local_input_cb))
self.clicked_input_sub = rospy.Subscriber("/face_adls/clicked_move", PoseStamped,
async_call(self.global_input_cb))
self.feedback_pub = rospy.Publisher('/face_adls/feedback', String)
self.global_move_poses_pub = rospy.Publisher('/face_adls/global_move_poses',
StringArray, latch=True)
self.controller_enabled_pub = rospy.Publisher('/face_adls/controller_enabled',
Bool, latch=True)
self.enable_controller_srv = rospy.Service("/face_adls/enable_controller",
EnableFaceController, self.enable_controller_cb)
self.request_registration = rospy.ServiceProxy("/request_registration", RequestRegistration)
self.enable_mpc_srv = rospy.ServiceProxy('/haptic_mpc/enable_mpc', EnableHapticMPC)
self.ell_params_pub = rospy.Publisher("/ellipsoid_params", EllipsoidParams, latch=True)
def stop_move_cb(msg):
self.stop_moving()
self.stop_move_sub = rospy.Subscriber("/face_adls/stop_move", Bool, stop_move_cb, queue_size=1)
def stop_moving(self):
"""Send empty PoseArray to skin controller to stop movement"""
for x in range(2):
self.pose_traj_pub.publish(PoseArray()) #Send empty msg to skin controller
cart_traj_msg = [PoseConv.to_pose_msg(self.current_ee_pose(self.ee_frame))]
head = Header()
head.frame_id = '/torso_lift_link'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
rospy.loginfo( "Sent stop moving commands!")
def register_ellipse(self, mode, side):
reg_e_params = EllipsoidParams()
try:
now = rospy.Time.now()
self.tfl.waitForTransform("/base_link", "/head_frame", now, rospy.Duration(10.))
pos, quat = self.tfl.lookupTransform("/head_frame", "/base_frame", now)
self.head_pose = PoseStamped()
self.head_pose.header.stamp = now
self.head_pose.header.frame_id = "/base_link"
self.head_pose.pose.position = Point(*pos)
self.head_pose.pose.orientation = Quaternion(*quat)
except:
rospy.logwarn("[%s] Head registration not loaded yet." %rospy.get_name())
return (False, reg_e_params)
reg_prefix = rospy.get_param("~registration_prefix", "")
registration_files = rospy.get_param("~registration_files", None)
if mode not in registration_files:
rospy.logerr("[%s] Mode not in registration_files parameters" % (rospy.get_name()))
return (False, reg_e_params)
try:
bag_str = reg_prefix + registration_files[mode][side]
rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str))
bag = rosbag.Bag(bag_str, 'r')
e_params = None
for topic, msg, ts in bag.read_messages():
e_params = msg
assert e_params is not None
bag.close()
except:
rospy.logerr("[%s] Cannot load registration parameters from %s" %(rospy.get_name(), bag_str))
return (False, reg_e_params)
head_reg_mat = PoseConv.to_homo_mat(self.head_pose)
ell_reg = PoseConv.to_homo_mat(Transform(e_params.e_frame.transform.translation,
e_params.e_frame.transform.rotation))
reg_e_params.e_frame = PoseConv.to_tf_stamped_msg(head_reg_mat**-1 * ell_reg)
reg_e_params.e_frame.header.frame_id = self.head_reg_tf.header.frame_id
reg_e_params.e_frame.child_frame_id = '/ellipse_frame'
reg_e_params.height = e_params.height
reg_e_params.E = e_params.E
self.ell_params_pub.publish(reg_e_params)
self.ell_frame = reg_e_params.e_frame
return (True, reg_e_params)
def current_ee_pose(self, link, frame='/torso_lift_link'):
"""Get current end effector pose from tf"""
# print "Getting pose of %s in frame: %s" %(link, frame)
try:
now = rospy.Time.now()
self.tfl.waitForTransform(frame, link, now, rospy.Duration(8.0))
pos, quat = self.tfl.lookupTransform(frame, link, now)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[face_adls_manager] TF Failure getting current end-effector pose: %s" %e)
return None
return pos, quat
def publish_feedback(self, message=None):
if message is not None:
rospy.loginfo("[face_adls_manager] %s" % message)
self.feedback_pub.publish(message)
def enable_controller_cb(self, req):
if req.enable:
face_adls_modes = rospy.get_param('/face_adls_modes', None)
params = face_adls_modes[req.mode]
self.face_side = rospy.get_param('/face_side', 'r') # TODO Make more general
self.trim_retreat = req.mode == "shaving"
self.flip_gripper = self.face_side == 'r' and req.mode == "shaving"
bounds = params['%s_bounds' % self.face_side]
self.ellipsoid.set_bounds(bounds['lat'], bounds['lon'], bounds['height'])#TODO: Change Back
#self.ellipsoid.set_bounds((-np.pi,np.pi), (-np.pi,np.pi), (0,100))
success, e_params = self.register_ellipse(req.mode, self.face_side)
if not success:
self.publish_feedback(Messages.NO_PARAMS_LOADED)
return EnableFaceControllerResponse(False)
reg_pose = PoseConv.to_pose_stamped_msg(e_params.e_frame)
try:
now = rospy.Time.now()
reg_pose.header.stamp = now
self.tfl.waitForTransform(reg_pose.header.frame_id, '/base_link',
now, rospy.Duration(8.0))
ellipse_frame_base = self.tfl.transformPose('/base_link', reg_pose)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[face_adls_manager] TF Failure converting ellipse to base frame: %s" %e)
self.ellipsoid.load_ell_params(ellipse_frame_base, e_params.E,
e_params.is_oblate, e_params.height)
global_poses_dir = rospy.get_param("~global_poses_dir", "")
global_poses_file = params["%s_ell_poses_file" % self.face_side]
global_poses_resolved = roslaunch.substitution_args.resolve_args(
global_poses_dir + "/" + global_poses_file)
self.global_poses = rosparam.load_file(global_poses_resolved)[0][0]
self.global_move_poses_pub.publish(sorted(self.global_poses.keys()))
#Check rotating gripper based on side of body
if self.flip_gripper:
self.gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0)
print "Rotating gripper by PI around x-axis"
else:
self.gripper_rot = trans.quaternion_from_euler(0, 0, 0)
print "NOT Rotating gripper around x-axis"
# check if arm is near head
# cart_pos, cart_quat = self.current_ee_pose(self.ee_frame)
# ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
# equals = ell_pos == self.ellipsoid.enforce_bounds(ell_pos)
# print ell_pos, equals
# if self.ellipsoid._lon_bounds[0] >= 0 and ell_pos[1] >= 0:
# arm_in_bounds = np.all(equals)
# else:
# ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi)
# min_lon = np.mod(self.ellipsoid._lon_bounds[0], 2 * np.pi)
# arm_in_bounds = (equals[0] and
# equals[2] and
# (ell_lon_1 >= min_lon or ell_lon_1 <= self.ellipsoid._lon_bounds[1]))
arm_in_bounds = True
self.setup_force_monitor()
success = True
if not arm_in_bounds:
self.publish_feedback(Messages.ARM_AWAY_FROM_HEAD)
success = False
#Switch back to l_arm_controller if necessary
if self.controller_switcher.carefree_switch('l', '%s_arm_controller', reset=False):
print "Controller switch to l_arm_controller succeeded"
self.publish_feedback(Messages.ENABLE_CONTROLLER)
else:
print "Controller switch to l_arm_controller failed"
success = False
self.controller_enabled_pub.publish(Bool(success))
req = EnableHapticMPCRequest()
req.new_state = 'enabled'
resp = self.enable_mpc_srv.call(req)
else:
self.stop_moving()
self.controller_enabled_pub.publish(Bool(False))
rospy.loginfo(Messages.DISABLE_CONTROLLER)
req = EnableHapticMPCRequest()
req.new_state = 'disabled'
self.enable_mpc_srv.call(req)
success = False
return EnableFaceControllerResponse(success)
def setup_force_monitor(self):
self.force_monitor = ForceCollisionMonitor()
# registering force monitor callbacks
def dangerous_cb(force):
self.stop_moving()
curr_pose = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
ell_pos, _ = self.ellipsoid.pose_to_ellipsoidal(curr_pose)
if ell_pos[2] < SAFETY_RETREAT_HEIGHT * 0.9:
self.publish_feedback(Messages.DANGEROUS_FORCE %force)
self.retreat_move(SAFETY_RETREAT_HEIGHT,
SAFETY_RETREAT_VELOCITY,
forced=True)
self.force_monitor.register_dangerous_cb(dangerous_cb)
def timeout_cb(time):
start_time = rospy.get_time()
ell_pos, _ = self.ellipsoid.pose_to_ellipsoidal(self.current_ee_pose(self.ee_frame, '/ellipse_frame'))
rospy.loginfo("ELL POS TIME: %.3f " % (rospy.get_time() - start_time) + str(ell_pos)
+ " times: %.3f %.3f" % (self.force_monitor.last_activity_time, rospy.get_time()))
if ell_pos[2] < RETREAT_HEIGHT * 0.9 and self.force_monitor.is_inactive():
self.publish_feedback(Messages.TIMEOUT_RETREAT % time)
self.retreat_move(RETREAT_HEIGHT, LOCAL_VELOCITY)
self.force_monitor.register_timeout_cb(timeout_cb)
def contact_cb(force):
self.force_monitor.update_activity()
if not self.is_forced_retreat:
self.stop_moving()
self.publish_feedback(Messages.CONTACT_FORCE % force)
rospy.loginfo("[face_adls_manZger] Arm stopped due to making contact.")
self.force_monitor.register_contact_cb(contact_cb)
self.force_monitor.start_activity()
self.force_monitor.update_activity()
self.is_forced_retreat = False
def retreat_move(self, height, velocity, forced=False):
if forced:
self.is_forced_retreat = True
cart_pos, cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
#print "Retreat EP:", ell_pos
latitude = ell_pos[0]
if self.trim_retreat:
latitude = min(latitude, TRIM_RETREAT_LATITUDE)
#rospy.loginfo("[face_adls_manager] Retreating from current location.")
retreat_pos = [latitude, ell_pos[1], height]
retreat_pos = self.ellipsoid.enforce_bounds(retreat_pos)
retreat_quat = [0,0,0,1]
if forced:
cart_path = self.ellipsoid.ellipsoidal_to_pose((retreat_pos, retreat_quat))
cart_msg = [PoseConv.to_pose_msg(cart_path)]
else:
ell_path = self.ellipsoid.create_ellipsoidal_path(ell_pos,
ell_quat,
retreat_pos,
retreat_quat,
velocity=0.01,
min_jerk=True)
cart_path = [self.ellipsoid.ellipsoidal_to_pose(ell_pose) for ell_pose in ell_path]
cart_msg = [PoseConv.to_pose_msg(pose) for pose in cart_path]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_msg)
self.pose_traj_pub.publish(pose_array)
self.is_forced_retreat = False
def global_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
rospy.loginfo("[face_adls_manager] Performing global move.")
if type(msg) == String:
self.publish_feedback(Messages.GLOBAL_START %msg.data)
goal_ell_pose = self.global_poses[msg.data]
elif type(msg) == PoseStamped:
try:
self.tfl.waitForTransform(msg.header.frame_id, '/ellipse_frame', msg.header.stamp, rospy.Duration(8.0))
goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg)
goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(goal_cart_pose)
flip_quat = trans.quaternion_from_euler(-np.pi/2, np.pi, 0)
goal_cart_quat_flipped = trans.quaternion_multiply(goal_cart_quat, flip_quat)
goal_cart_pose = PoseConv.to_pose_stamped_msg('/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped))
self.publish_feedback('Moving around ellipse to clicked position).')
goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(goal_cart_pose)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s" %e)
return
curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT]
retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot, [1.,0.,0.,0.])
approach_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT]
approach_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
final_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST]
final_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose((retreat_ell_pos, retreat_ell_quat))
cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_ret_pose)
cart_app_pose = self.ellipsoid.ellipsoidal_to_pose((approach_ell_pos, approach_ell_quat))
cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_app_pose)
cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose((final_ell_pos, final_ell_quat))
cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_goal_pose)
retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
retreat_ell_pos, retreat_ell_quat,
velocity=0.01, min_jerk=True)
transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(retreat_ell_pos, retreat_ell_quat,
approach_ell_pos, approach_ell_quat,
velocity=0.01, min_jerk=True)
approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(approach_ell_pos, approach_ell_quat,
final_ell_pos, final_ell_quat,
velocity=0.01, min_jerk=True)
full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj
full_cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
ps = PoseStamped()
ps.header = head
ps.pose = cart_traj_msg[0]
self.force_monitor.update_activity()
def local_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
self.stop_moving()
button_press = msg.data
curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
if button_press in ell_trans_params:
self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
change_trans_ep = ell_trans_params[button_press]
goal_ell_pos = [curr_ell_pos[0]+change_trans_ep[0],
curr_ell_pos[1]+change_trans_ep[1],
curr_ell_pos[2]+change_trans_ep[2]]
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
goal_ell_pos, curr_ell_quat,
velocity=ELL_LOCAL_VEL, min_jerk=True)
elif button_press in ell_rot_params:
self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
change_rot_ep = ell_rot_params[button_press]
rot_quat = trans.quaternion_from_euler(*change_rot_ep)
goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat)
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
curr_ell_pos, goal_ell_quat,
velocity=ELL_ROT_VEL, min_jerk=True)
elif button_press == "reset_rotation":
self.publish_feedback(Messages.ROT_RESET_START)
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
curr_ell_pos,(0.,0.,0.,1.),
velocity=ELL_ROT_VEL, min_jerk=True)
else:
rospy.logerr("[face_adls_manager] Unknown local ellipsoidal command")
cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
self.force_monitor.update_activity()
def main():
rospy.init_node("test_ellipsoid_controller")
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
rospy.sleep(1)
joint_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
setup_angles = [
0, 0, np.pi / 2, -np.pi / 2, -np.pi, -np.pi / 2, -np.pi / 2
]
#joint_controller = EPArmController(joint_arm, 0.1, "joint_ep_controller")
#joint_controller.execute_interpolated_ep(setup_angles, 10)
ctrl_switcher.carefree_switch('l', '%s_cart', None)
rospy.sleep(1)
cart_arm = create_pr2_arm('l',
PR2ArmJTransposeTask,
end_link="%s_gripper_shaver45_frame")
cart_arm.set_posture(setup_angles)
#cart_arm.set_posture(cart_arm.get_joint_angles(wrapped=False))
cart_arm.set_gains([110, 600, 600, 40, 40, 40],
[15, 15, 15, 1.2, 1.2, 1.2])
ell_controller = EllipsoidController(cart_arm)
ell_controller.reset_arm_orientation(8)
pg.init()
screen = pg.display.set_mode((300, 300))
while not rospy.is_shutdown():
pg.event.get()
keys = pg.key.get_pressed()
dur = 5
if True:
r = np.random.randint(6)
if r == 0:
ell_controller.command_move([1, 0, 0], dur)
elif r == 1:
ell_controller.command_move([-1, 0, 0], dur)
elif r == 2:
ell_controller.command_move([0, 1, 0], dur)
elif r == 3:
ell_controller.command_move([0, -1, 0], dur)
elif r == 4:
ell_controller.command_move([0, 0, 1], dur)
elif r == 5:
ell_controller.command_move([0, 0, -1], dur)
rospy.sleep(1)
else:
if keys[pg.K_w]:
ell_controller.command_move([-1, 0, 0], dur)
if keys[pg.K_s]:
ell_controller.command_move([1, 0, 0], dur)
if keys[pg.K_a]:
ell_controller.command_move([0, 1, 0], dur)
if keys[pg.K_d]:
ell_controller.command_move([0, -1, 0], dur)
if keys[pg.K_q]:
ell_controller.command_move([0, 0, 1], dur)
if keys[pg.K_e]:
ell_controller.command_move([0, 0, -1], dur)
rospy.sleep(0.05)
def main():
from optparse import OptionParser
p = OptionParser()
p.add_option('-f', '--filename', dest="filename", default="",
help="File to save trajectory to or load from.")
p.add_option('-l', '--left_arm', dest="left_arm",
action="store_true", default=False,
help="Use left arm.")
p.add_option('-r', '--right_arm', dest="right_arm",
action="store_true", default=False,
help="Use right arm.")
p.add_option('-s', '--save_mode', dest="save_mode",
action="store_true", default=False,
help="Saving mode.")
p.add_option('-t', '--traj_mode', dest="traj_mode",
action="store_true", default=False,
help="Trajectory mode.")
p.add_option('-c', '--ctrl_name', dest="ctrl_name", default=CTRL_NAME_LOW,
help="Controller to run the playback with.")
p.add_option('-p', '--params', dest="param_file", default=None, #PARAM_FILE_LOW,
help="YAML file to save parameters in or load from.")
p.add_option('-v', '--srv_mode', dest="srv_mode",
action="store_true", default=False,
help="Server mode.")
p.add_option('-y', '--playback_mode', dest="playback_mode",
action="store_true", default=False,
help="Plays back trajectory immediately.")
p.add_option('-z', '--reverse', dest="reverse",
action="store_true", default=False,
help="Plays back trajectory in reverse.")
(opts, args) = p.parse_args()
if opts.right_arm:
arm_char = 'r'
elif opts.left_arm:
arm_char = 'l'
else:
print "Must select arm with -r or -l."
return
filename = opts.filename
rospy.init_node("arm_pose_move_controller_%s" % arm_char)
if opts.save_mode:
assert(opts.right_arm + opts.left_arm == 1)
if opts.traj_mode:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch(arm_char, CTRL_NAME_NONE, PARAM_FILE_NONE, reset=False)
traj_saver = TrajectorySaver(RATE, CTRL_NAME_NONE)
raw_input("Press enter to start recording")
traj_saver.record_trajectory(arm_char, blocking=False)
import curses
def wait_for_key(stdscr):
curses.init_pair(1, curses.COLOR_RED, curses.COLOR_WHITE)
stdscr.addstr(0,0, "Press any key to stop!", curses.color_pair(1) )
stdscr.refresh()
c = 0
while not rospy.is_shutdown() and c == 0:
c = stdscr.getch()
curses.wrapper(wait_for_key)
traj_saver.stop_record(roslaunch.substitution_args.resolve_args(opts.filename))
ctrl_switcher.carefree_switch(arm_char, opts.ctrl_name, PARAM_FILE_LOW, reset=False)
return
else:
print "FIX"
return
elif opts.srv_mode:
traj_srv = TrajectoryServer(arm_char, "/trajectory_playback_" + arm_char,
opts.ctrl_name, opts.param_file)
rospy.spin()
return
elif opts.playback_mode:
raw_input("Press enter to continue")
if opts.traj_mode:
exec_traj_from_file(opts.filename,
ctrl_name=opts.ctrl_name,
param_file=opts.param_file,
reverse=opts.reverse,
blocking=True)
else:
move_to_setup_from_file(opts.filename,
ctrl_name=opts.ctrl_name,
param_file=opts.param_file,
reverse=opts.reverse,
blocking=True)
class CartesianControllerManager(object):
def __init__(self, arm_char):
self.arm_char = arm_char
self.arm = None
self.kin = None
self.cart_ctrl = CartesianStepController()
self.ctrl_switcher = ControllerSwitcher()
self.command_move_sub = rospy.Subscriber(
"/face_adls/%s_cart_move" % arm_char, TwistStamped,
async_call(self.command_move_cb))
self.command_absolute_sub = rospy.Subscriber(
"/face_adls/%s_cart_absolute" % arm_char, PoseStamped,
async_call(self.command_absolute_cb))
self.adjust_posture_sub = rospy.Subscriber(
"/face_adls/%s_cart_posture" % arm_char, Float32,
self.adjust_posture_cb)
def enable_controller_cb(req):
if req.enable:
_, frame_rot = PoseConv.to_pos_rot(
[0] * 3,
[req.frame_rot.x, req.frame_rot.y, req.frame_rot.z])
if req.velocity == 0:
req.velocity = 0.03
success = self.enable_controller(req.end_link,
req.ctrl_params,
req.ctrl_name,
frame_rot,
velocity=req.velocity)
else:
success = self.disable_controller()
return EnableCartControllerResponse(success)
self.controller_enabled_pub = rospy.Publisher(
'/face_adls/%s_cart_ctrl_enabled' % arm_char, Bool, latch=True)
self.enable_controller_srv = rospy.Service(
"/face_adls/%s_enable_cart_ctrl" % arm_char, EnableCartController,
enable_controller_cb)
def enable_controller(
self,
end_link="%s_gripper_tool_frame",
ctrl_params="$(find hrl_face_adls)/params/%s_jt_task_tool.yaml",
ctrl_name="%s_cart_jt_task_tool",
frame_rot=FLIP_PERSPECTIVE_ROT,
velocity=0.03):
#frame_rot=np.mat(np.eye(3))):
rospy.loginfo(
"[cartesian_manager] Enabling %s controller with end link %s" %
(ctrl_name, end_link))
try:
if '%s' in end_link:
end_link = end_link % self.arm_char
if '%s' in ctrl_params:
ctrl_params = ctrl_params % self.arm_char
if '%s' in ctrl_name:
ctrl_name = ctrl_name % self.arm_char
self.ctrl_switcher.carefree_switch(self.arm_char,
ctrl_name,
ctrl_params,
reset=False)
rospy.sleep(0.2)
cart_arm = create_ep_arm(self.arm_char,
PR2ArmJTransposeTask,
controller_name=ctrl_name,
end_link=end_link,
timeout=5)
self.cart_ctrl.set_arm(cart_arm)
self.arm = cart_arm
rospy.sleep(0.1)
self.cur_posture_angle = self.arm.get_joint_angles()[2]
self.adjust_posture_cb(Float32(0.))
self.frame_rot = frame_rot
self.velocity = velocity
self.controller_enabled_pub.publish(True)
except Exception as e:
print e
return False
rospy.loginfo(
"[cartesian_manager] Successfully enabled %s controller with end link %s"
% (ctrl_name, end_link))
return True
def disable_controller(self):
rospy.loginfo("[cartesian_manager] Disabling cartesian controller.")
self.cart_ctrl.set_arm(None)
self.arm = None
self.controller_enabled_pub.publish(False)
return True
def command_move_cb(self, msg):
if self.arm is None:
rospy.logwarn(
"[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names(
):
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = PoseConv.to_pos_rot(self.arm.get_ep())
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
_, torso_rot_ref = PoseConv.to_pos_rot(torso_B_ref)
torso_rot_ref *= self.frame_rot
ref_pos_off, ref_rot_off = PoseConv.to_pos_rot(msg)
change_pos = torso_rot_ep.T * torso_rot_ref * ref_pos_off
change_pos_xyz = change_pos.T.A[0]
ep_rot_ref = torso_rot_ep.T * torso_rot_ref
change_rot = ep_rot_ref * ref_rot_off * ep_rot_ref.T
_, change_rot_rpy = PoseConv.to_pos_euler(
np.mat([0] * 3).T, change_rot)
rospy.loginfo("[cartesian_manager] Command relative movement." +
str((change_pos_xyz, change_rot_rpy)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot_rpy),
((0, 0, 0), 0),
velocity=self.velocity,
blocking=True)
def command_absolute_cb(self, msg):
if self.arm is None:
rospy.logwarn(
"[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names(
):
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = self.arm.get_ep()
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
ref_B_goal = PoseConv.to_homo_mat(msg)
torso_B_goal = torso_B_ref * ref_B_goal
change_pos, change_rot = PoseConv.to_pos_rot(torso_B_goal)
change_pos_xyz = change_pos.T.A[0]
rospy.loginfo("[cartesian_manager] Command absolute movement." +
str((change_pos_xyz, change_rot)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot),
((1, 1, 1), 1),
velocity=self.velocity,
blocking=True)
def adjust_posture_cb(self, msg):
rospy.loginfo("[cartesian_manager] Adjusting posture")
new_posture = [None] * 7
self.cur_posture_angle += msg.data
new_posture[2] = self.cur_posture_angle
self.arm.set_posture(new_posture)
