From 5dec76bd8794d5a9e8c82205ec17960424268dd7 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Tue, 13 Mar 2018 17:05:55 -0400 Subject: [PATCH 1/3] update tio add extra debugging info --- .../src/costar_robot/costar_arm.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py b/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py index 86dd3f1e..7bc2c775 100644 --- a/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py +++ b/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py @@ -155,6 +155,7 @@ def __init__(self, self.status_pub = self.make_pub('DriverStatus',String,queue_size=1000) self.info_pub = self.make_pub('info',String,queue_size=1000) + self.object_pub = self.make_pub('SmartMove/object',String,queue_size=1000) self.display_pub = self.make_pub('display_trajectory',DisplayTrajectory,queue_size=1000) self.robot = URDF.from_parameter_server() @@ -437,8 +438,9 @@ def send_trajectory(self,traj,stamp,acceleration=0.5,velocity=0.5,cartesian=Fals rospy.logerr("Function 'send_trajectory' not implemented for base class!") return "FAILURE -- running base class!" - def info(self, msg): + def info(self, msg, object_name): self.info_pub.publish(data=msg) + self.object_pub.publish(data=object_name) ''' Standard movement call. @@ -1006,17 +1008,17 @@ def smartmove_multipurpose_gripper(self, if dist > 2 * backup_dist: rospy.logwarn("Backoff failed for pose %i: distance was %f vs %f"%(sequence_number,dist,2*backup_dist)) continue - self.info("appoach_%s"%obj) + self.info("appoach_%s"%obj, obj) msg = self.send_and_publish_planning_result(res,stamp,acceleration,velocity) rospy.sleep(0.1) if msg[0:7] == 'SUCCESS': - self.info("move_to_grasp_%s"%obj) + self.info("move_to_grasp_%s"%obj, obj) msg = self.send_and_publish_planning_result(res2,stamp,acceleration,velocity) rospy.sleep(0.1) if msg[0:7] == 'SUCCESS': - self.info("take_%s"%obj) + self.info("take_%s"%obj, obj) gripper_function(obj) traj = res2.planned_trajectory.joint_trajectory @@ -1030,7 +1032,7 @@ def smartmove_multipurpose_gripper(self, traj.points[0].positions = self.q0 traj.points[-1].velocities = [0.]*len(self.q0) - self.info("backoff_from_%s"%obj) + self.info("backoff_from_%s"%obj, obj) msg = self.send_and_publish_planning_result(res2,stamp,acceleration,velocity) return msg else: From 3ad6848ccf1ccd997ad1ba8b3e04fa82d1550fa3 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Tue, 13 Mar 2018 21:09:34 -0400 Subject: [PATCH 2/3] joint space moves --- .../src/costar_robot/costar_arm.py | 47 +++++++++++++++++-- 1 file changed, 42 insertions(+), 5 deletions(-) diff --git a/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py b/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py index 7bc2c775..39d2dee3 100644 --- a/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py +++ b/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py @@ -137,6 +137,7 @@ def __init__(self, self.plan_home_srv = self.make_service('PlanToHome',ServoToPose,self.plan_to_home_cb) self.smartmove = self.make_service('SmartMove',SmartMove,self.smart_move_cb) self.js_servo = self.make_service('ServoToJointState',ServoToJointState,self.servo_to_joints_cb) + self.js_plan = self.make_service('PlanToJointState',ServoToJointState,self.plan_to_joints_cb) self.smartmove_release_srv = self.make_service('SmartRelease',SmartMove,self.smartmove_release_cb) self.smartmove_grasp_srv = self.make_service('SmartGrasp',SmartMove,self.smartmove_grasp_cb) self.smartmove_grasp_srv = self.make_service('SmartPlace',SmartMove,self.smartmove_place_cb) @@ -516,16 +517,52 @@ def servo_to_home_cb(self,req): rospy.logerr('SIMPLE DRIVER -- Not in servo mode') return 'FAILURE -- not in servo mode' - ''' - Standard move call. - Make a joint space move to a destination. - ''' def servo_to_joints_cb(self,req): + if self.driver_status == 'SERVO': + + # Check acceleration and velocity limits + (acceleration, velocity) = self.check_req_speed_params(req) + + stamp = self.acquire() + + # inverse kinematics + traj = self.planner.getJointMove(req.target.position, + self.q0, + self.base_steps, + self.steps_per_meter, + self.steps_per_radians, + time_multiplier = (1./velocity), + percent_acc = acceleration, + use_joint_move = True, + table_frame = self.table_pose) + + # Send command + if len(traj.points) > 0: + if stamp is not None: + rospy.logwarn("Robot moving to " + str(traj.points[-1].positions)) + res = self.send_trajectory(traj,stamp,acceleration,velocity,cartesian=False) + self.release() + else: + res = 'FAILURE -- could not preempt current arm control.' + return res + else: + rospy.logerr('SIMPLE DRIVER -- IK failed') + return 'FAILURE -- no trajectory points' + else: + rospy.logerr('SIMPLE DRIVER -- Not in servo mode') + return 'FAILURE -- not in servo mode' + + def plan_to_joints_cb(self,req): if self.driver_status == 'SERVO': # Check acceleration and velocity limits (acceleration, velocity) = self.check_req_speed_params(req) - return 'FAILURE -- not yet implemented!' + stamp = self.acquire() + + # Send command + pt = JointTrajectoryPoint() + (code,res) = self.planner.getPlan(q_goal=req.target.position,q=self.q0) + return self.send_and_publish_planning_result(res,stamp,acceleration,velocity) else: rospy.logerr('SIMPLE DRIVER -- Not in servo mode') From edbd117ff84c89968eba1632977e77bb3b25ccd5 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Tue, 13 Mar 2018 21:18:26 -0400 Subject: [PATCH 3/3] update fail messages --- .../costar_robot_manager/src/costar_robot/costar_arm.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py b/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py index 39d2dee3..942c7ff0 100644 --- a/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py +++ b/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py @@ -1142,7 +1142,7 @@ def smartmove_release_cb(self, req): stamp = self.acquire() list_of_waypoints = self.query(req, True) if len(list_of_waypoints) == 0: - return "FAILURE -- no suitable points found" + return "FAILURE -- no suitable waypoints found for release" distance = req.backoff return self.smartmove_release(stamp, list_of_waypoints, distance, req.vel, req.accel) @@ -1153,7 +1153,7 @@ def smartmove_grasp_cb(self, req): stamp = self.acquire() list_of_waypoints = self.query(req, True) if len(list_of_waypoints) == 0: - return "FAILURE -- no suitable points found" + return "FAILURE -- no suitable waypoints found for grasp" distance = req.backoff return self.smartmove_grasp(stamp, list_of_waypoints, distance, req.vel, req.accel)