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)