def get_fk(self, angles, frame='torso_lift_link', link=-1): # get FK pose of a list of joint angles for the arm fk_request = GetPositionFKRequest() fk_request.header.frame_id = frame fk_request.header.stamp = rospy.Time.now() fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names fk_request.robot_state.joint_state.header = fk_request.header fk_request.robot_state.joint_state.position = angles #self.joint_state_act.positions fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names print 'fk_request:', fk_request try: return self.fk_pose_proxy(fk_request).pose_stamped[link] except rospy.ServiceException, e: rospy.loginfo("FK service did not process request: %s" %str(e))
def get_fk(self, msg, frame='/torso_lift_link'): # get FK pose of a list of joint angles for the arm fk_request = GetPositionFKRequest() fk_request.header.frame_id = frame #fk_request.header.stamp = rospy.Time.now() fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names fk_request.robot_state.joint_state.position = self.joint_state_act.positions fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names #print "FK Request: %s " %fk_request try: fk_response = self.fk_pose_proxy(fk_request) return fk_response.pose_stamped[-1] except rospy.ServiceException, e: rospy.loginfo("FK service did not process request: %s" %str(e))
def _setup_fk(self): '''Sets up services for forward kinematics.''' side = self._side() # Get FK info service. fk_info_srv_name = PR2_SERVICE_PREFIX + side + SERVICE_FK_INFO_POSTFIX rospy.wait_for_service(fk_info_srv_name) fk_info_srv = rospy.ServiceProxy(fk_info_srv_name, GetKinematicSolverInfo) ks_info = fk_info_srv().kinematic_solver_info rospy.loginfo('FK info service has responded for ' + side + ' arm.') # Get FK service. fk_srv_name = PR2_SERVICE_PREFIX + side + SERVICE_FK_POSTFIX rospy.wait_for_service(fk_srv_name) self.fk_srv = rospy.ServiceProxy(fk_srv_name, GetPositionFK, persistent=True) rospy.loginfo('FK service has responded for ' + side + ' arm.') # Set up common parts of an FK request. self.fk_request = GetPositionFKRequest() self.fk_request.header.frame_id = BASE_LINK self.fk_request.fk_link_names = ks_info.link_names self.fk_request.robot_state.joint_state.name = ks_info.joint_names
def __init__(self): rospy.init_node("test_robot_get_fk") self.joint_state = JointState() self.rate = 1 r = rospy.Rate(self.rate) q = quaternion_from_euler(0, 0, 0) #rospy.loginfo(q) rospy.wait_for_service('test_arm_kinematics/get_fk') rospy.wait_for_service('test_arm_kinematics/get_fk_solver_info') get_fk_proxy = rospy.ServiceProxy('test_arm_kinematics/get_fk', GetPositionFK, persistent=True) get_fk_solver_info_proxy = rospy.ServiceProxy( 'test_arm_kinematics/get_fk_solver_info', GetKinematicSolverInfo) left_arm_solver_info = get_fk_solver_info_proxy() rospy.Subscriber('joint_states', JointState, self.getJointState) self.request = GetPositionFKRequest() self.request.robot_state.joint_state = JointState() self.request.robot_state.joint_state.header.frame_id = 'base_actuator' self.request.robot_state.joint_state.name = left_arm_solver_info.kinematic_solver_info.joint_names self.request.robot_state.joint_state.position = [0] * len( self.request.robot_state.joint_state.name) self.request.header.frame_id = "base_actuator" self.request.fk_link_names = list() self.request.fk_link_names.append("base_actuator") self.request.fk_link_names.append("link1") self.request.fk_link_names.append("link2") self.request.fk_link_names.append("link3") self.request.fk_link_names.append("link4") self.request.fk_link_names.append("link5") self.request.fk_link_names.append("link6") self.request.fk_link_names.append("link7") self.request.fk_link_names.append("link8") self.request.fk_link_names.append("link9") while not rospy.is_shutdown(): try: response = get_fk_proxy(self.request) hand_link = response.pose_stamped[0] q = list() q.append(hand_link.pose.orientation.x) q.append(hand_link.pose.orientation.y) q.append(hand_link.pose.orientation.z) q.append(hand_link.pose.orientation.w) rpy = euler_from_quaternion(q) rospy.loginfo(rpy) rospy.loginfo(response.pose_stamped) except rospy.ServiceException, e: print "Service call failed: %s" % e r.sleep()
def FK(self, arm, q): fk_req = GetPositionFKRequest() fk_req.header.frame_id = 'torso_lift_link' if arm == 0: fk_req.fk_link_names.append('r_wrist_roll_link') else: fk_req.fk_link_names.append('l_wrist_roll_link') fk_req.robot_state.joint_state.name = self.joint_names_list[arm] fk_req.robot_state.joint_state.position = q fk_resp = self.fk_srv[arm].call(fk_req) if fk_resp.error_code.val == fk_resp.error_code.SUCCESS: x = fk_resp.pose_stamped[0].pose.position.x y = fk_resp.pose_stamped[0].pose.position.y z = fk_resp.pose_stamped[0].pose.position.z pos = [x, y, z] q1 = fk_resp.pose_stamped[0].pose.orientation.x q2 = fk_resp.pose_stamped[0].pose.orientation.y q3 = fk_resp.pose_stamped[0].pose.orientation.z q4 = fk_resp.pose_stamped[0].pose.orientation.w quat = [q1,q2,q3,q4] rot = tr.quaternion_to_matrix(quat) else: rospy.logerr('Forward kinematics failed') return None, None return pos, rot
def __init__(self, side_prefix, gui): self.side_prefix = side_prefix self.gui = gui # Set up Inversse Kinematics services fk_info_srv_name = ('pr2_' + self._side() + '_arm_kinematics/get_fk_solver_info') fk_srv_name = 'pr2_' + self._side() + '_arm_kinematics/get_fk' rospy.loginfo('Waiting for FK info service to respond.') rospy.wait_for_service(fk_info_srv_name) fk_info_srv = rospy.ServiceProxy(fk_info_srv_name, GetKinematicSolverInfo) solver_info = fk_info_srv() self.fk_joints = solver_info.kinematic_solver_info.joint_names self.fk_limits = solver_info.kinematic_solver_info.limits print(solver_info) rospy.loginfo('Waiting for FK service to respond.') rospy.wait_for_service(fk_srv_name) self.fk_srv = rospy.ServiceProxy(fk_srv_name, GetPositionFK, persistent=True) # Set up common parts of an FK request self.fk_request = GetPositionFKRequest() # self.fk_request.timeout = rospy.Duration(4.0) self.fk_request.fk_link_names = solver_info.kinematic_solver_info.link_names
def get_fk(self, angles, frame='torso_lift_link', link=-1): # get FK pose of a list of joint angles for the arm fk_request = GetPositionFKRequest() fk_request.header.frame_id = frame fk_request.header.stamp = rospy.Time.now() fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names fk_request.robot_state.joint_state.header = fk_request.header fk_request.robot_state.joint_state.position = angles #self.joint_state_act.positions fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names print 'fk_request:', fk_request try: return self.fk_pose_proxy(fk_request).pose_stamped[link] except rospy.ServiceException, e: rospy.loginfo("FK service did not process request: %s" % str(e))
def FK(self, arm, q): if rospy.is_shutdown(): sys.exit() if arm != 1: arm = 0 fk_req = GetPositionFKRequest() fk_req.header.frame_id = 'torso_lift_link' if arm == 0: fk_req.fk_link_names.append('r_wrist_roll_link') else: fk_req.fk_link_names.append('l_wrist_roll_link') fk_req.robot_state.joint_state.name = self.joint_names_list[arm] fk_req.robot_state.joint_state.position = q fk_resp = GetPositionFKResponse() fk_resp = self.fk_srv[arm].call(fk_req) if fk_resp.error_code.val == fk_resp.error_code.SUCCESS: x = fk_resp.pose_stamped[0].pose.position.x y = fk_resp.pose_stamped[0].pose.position.y z = fk_resp.pose_stamped[0].pose.position.z q1 = fk_resp.pose_stamped[0].pose.orientation.x q2 = fk_resp.pose_stamped[0].pose.orientation.y q3 = fk_resp.pose_stamped[0].pose.orientation.z q4 = fk_resp.pose_stamped[0].pose.orientation.w quat = [q1, q2, q3, q4] # Transform point from wrist roll link to actuator ret1 = self.transform_in_frame([x, y, z], quat, self.off_point) ret2 = np.matrix(quat).T else: rospy.logerr('Forward kinematics failed') ret1, ret2 = None, None return ret1, ret2
def get_fk(self, msg): #print "get_fk of %s" %str(msg) if (self.joint_state): fk_request = GetPositionFKRequest() fk_request.header.frame_id = '/torso_lift_link' fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names fk_request.robot_state.joint_state.position = self.joint_state fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names else: rospy.loginfo("No Joint States Available Yet") try: self.curr_pose = self.fk_pose_proxy(fk_request) self.pose_out.publish(self.curr_pose.pose_stamped[-1]) # print "Pose from FK: %s" %str(self.curr_pose.pose_stamped[-1]) except rospy.ServiceException, e: rospy.loginfo("FK service did not process request: %s" % str(e))
def get_fk(self, msg, frame='/torso_lift_link' ): # get FK pose of a list of joint angles for the arm fk_request = GetPositionFKRequest() fk_request.header.frame_id = frame #fk_request.header.stamp = rospy.Time.now() fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names fk_request.robot_state.joint_state.position = self.joint_state_act.positions fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names #print "FK Request: %s " %fk_request try: fk_response = self.fk_pose_proxy(fk_request) return fk_response.pose_stamped[-1] except rospy.ServiceException, e: rospy.loginfo("FK service did not process request: %s" % str(e))
def get_fk(self, msg): #print "get_fk of %s" %str(msg) if (self.joint_state): fk_request = GetPositionFKRequest() fk_request.header.frame_id = '/torso_lift_link' fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names fk_request.robot_state.joint_state.position = self.joint_state fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names else: rospy.loginfo("No Joint States Available Yet") try: self.curr_pose = self.fk_pose_proxy(fk_request) self.pose_out.publish(self.curr_pose.pose_stamped[-1]) # print "Pose from FK: %s" %str(self.curr_pose.pose_stamped[-1]) except rospy.ServiceException, e: rospy.loginfo("FK service did not process request: %s" %str(e))
def get_fk(self, fk_links, robot_state=None, frame_id=None): ''' Solves forward kinematics. In general, it is better to use the planning scene or the state transformer get_transform function between robot links rather than this function. **Args:** **fk_links ([string]):** Links for which you want FK solutions *robot_state (arm_navigation_msgs.mgs.RobotState):* The state of the robot during forward kinematics. If no state is passed in the state in the planning scene will be used. *frame_id (string):* The frame ID in which you want the returned poses. Note that the FK service may use TF so we recommend only using the robot frame. If no frame is specified, the robot frame is used. **Returns:** A list of geometry_msgs.msg.PoseStamped corresponding to forward kinematic solutions for fk_links. **Raises:** **rospy.ServiceException:** if there is a problem with the service call **Warning:** Calls a service which may use TF! Recommended that you only ask for poses in the robot frame. ''' req = GetPositionFKRequest() req.header.frame_id = frame_id if not frame_id: req.header.frame_id = self._psi.robot_frame req.header.stamp = rospy.Time(0) req.robot_state = robot_state if not robot_state: req.robot_state = self._psi.get_robot_state() req.fk_link_names = fk_links res = self._fk_service(req) if res.error_code.val != res.error_code.SUCCESS or not res.pose_stamped: raise ArmNavError('Forward kinematics failed', error_code=res.error_code) return res.pose_stamped
def __init__(self): rospy.init_node("fanuc_robot_get_fk") self.rate = 1 r = rospy.Rate(self.rate) self.joints=[] self.links=[] rospy.wait_for_service('get_fk') rospy.wait_for_service('get_fk_solver_info') get_fk_proxy = rospy.ServiceProxy('get_fk', GetPositionFK, persistent=True) get_fk_solver_info_proxy = rospy.ServiceProxy('get_fk_solver_info', GetKinematicSolverInfo) solver_info = get_fk_solver_info_proxy() rospy.loginfo(solver_info) for joint in solver_info.kinematic_solver_info.joint_names: self.joints.append(joint) rospy.loginfo("Adding joint " + str(joint)) for link in solver_info.kinematic_solver_info.link_names: self.links.append(link) rospy.loginfo("Adding link " + str(link)) self.request.robot_state.joint_state = JointState() self.request.robot_state.joint_state.header.frame_id = 'torso_link' self.request.robot_state.joint_state.name = self.joints self.request.robot_state.joint_state.position = [0] * len(self.joints) self.request.robot_state.joint_state.position[0] = 1.0 self.request = GetPositionFKRequest() self.request.fk_link_names = self.links self.request.header.frame_id = "torso_link" while not rospy.is_shutdown(): try: response = get_fk_proxy(self.request) rospy.loginfo(response) except rospy.ServiceException, e: print "Service call failed: %s" % e r.sleep()
def FK(self, arm, q): rospy.logwarn('Currently ignoring the arm parameter.') fk_req = GetPositionFKRequest() fk_req.header.frame_id = 'torso_lift_link' fk_req.fk_link_names.append('r_wrist_roll_link') fk_req.robot_state.joint_state.name = self.joint_names_list fk_req.robot_state.joint_state.position = q fk_resp = GetPositionFKResponse() fk_resp = self.fk_srv.call(fk_req) if fk_resp.error_code.val == fk_resp.error_code.SUCCESS: x = fk_resp.pose_stamped[0].pose.position.x y = fk_resp.pose_stamped[0].pose.position.y z = fk_resp.pose_stamped[0].pose.position.z ret = np.matrix([x, y, z]).T else: rospy.logerr('Forward kinematics failed') ret = None return ret
def FK(self, q): fk_req = GetPositionFKRequest() fk_req.header.frame_id = 'torso_lift_link' fk_req.fk_link_names.append(self.arm + '_wrist_roll_link') fk_req.robot_state.joint_state.name = self.joint_names_list fk_req.robot_state.joint_state.position = q fk_resp = self.fk_srv.call(fk_req) if fk_resp.error_code.val == fk_resp.error_code.SUCCESS: x = fk_resp.pose_stamped[0].pose.position.x y = fk_resp.pose_stamped[0].pose.position.y z = fk_resp.pose_stamped[0].pose.position.z pos = np.mat([x, y, z]).T q1 = fk_resp.pose_stamped[0].pose.orientation.x q2 = fk_resp.pose_stamped[0].pose.orientation.y q3 = fk_resp.pose_stamped[0].pose.orientation.z q4 = fk_resp.pose_stamped[0].pose.orientation.w quat = [q1, q2, q3, q4] mat = np.mat(tf_trans.quaternion_matrix(quat)) mat[:3, 3] = pos return mat rospy.logerr('Forward kinematics failed') return None
def push_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo("%s: preempted" % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback) start_angles = current_state.actual.positions full_state = rospy.wait_for_message("/joint_states", JointState) req = GetPositionFKRequest() req.header.frame_id = "base_link" req.fk_link_names = [GRIPPER_LINK_FRAME] req.robot_state.joint_state = full_state if not self.set_planning_scene_diff_client(): rospy.logerr("%s: failed to set planning scene diff" % ACTION_NAME) self.action_server.set_aborted() return pose = self.get_fk_srv(req).pose_stamped[0].pose frame_id = "base_link" approachpos = [pose.position.x, pose.position.y, pose.position.z] approachquat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] push_distance = 0.40 grasppos = [pose.position.x, pose.position.y - push_distance, pose.position.z] graspquat = [0.00000, 0.00000, 0.70711, -0.70711] # attach object to gripper, they will move together obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.02 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2] res = self.check_cartesian_path_lists( approachpos, approachquat, grasppos, graspquat, start_angles, frame=frame_id, ordered_collision_operations=ordered_collision_operations, ) error_code_dict = { ArmNavigationErrorCodes.SUCCESS: 0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1, ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2, ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3, ArmNavigationErrorCodes.PLANNING_FAILED: 4, } trajectory_len = len(res.trajectory.joint_trajectory.points) trajectory = [res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len)] vels = [res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len)] times = [res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len)] error_codes = [error_code_dict[error_code.val] for error_code in res.trajectory_error_codes] # if even one code is not 0, abort if sum(error_codes) != 0: for ind in range(len(trajectory)): rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) rospy.loginfo("") for ind in range(len(trajectory)): rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind])) rospy.logerr("%s: attempted push failed" % ACTION_NAME) self.action_server.set_aborted() return req = FilterJointTrajectoryRequest() req.trajectory = res.trajectory.joint_trajectory req.trajectory.points = req.trajectory.points[1:] # skip zero velocity point req.allowed_time = rospy.Duration(2.0) filt_res = self.trajectory_filter_srv(req) goal = FollowJointTrajectoryGoal() goal.trajectory = filt_res.trajectory goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id) rospy.sleep(0.5) self.trajectory_controller.send_goal(goal) self.trajectory_controller.wait_for_result() state = self.trajectory_controller.get_state() if state == GoalStatus.SUCCEEDED: rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded(PushObjectResult(sound_msg.recorded_sound)) return rospy.logerr("%s: attempted push failed" % ACTION_NAME) self.stop_audio_recording_srv(False) self.action_server.set_aborted()
def __init__(self): rospy.init_node("pr2lite_get_fk") self.joint_state = JointState() self.rate = 5 r = rospy.Rate(self.rate) q = quaternion_from_euler(0, 0, 0) hand_pub = rospy.Publisher('hand_pose', PoseStamped) side = "right" service_prefix = "pr2lite_" base_frame = "_arm_shelf_link" rospy.wait_for_service(service_prefix + side + '_arm_kinematics/get_fk') rospy.wait_for_service(service_prefix + side + '_arm_kinematics/get_fk_solver_info') get_fk_proxy = rospy.ServiceProxy(service_prefix + side + '_arm_kinematics/get_fk', GetPositionFK, persistent=True) get_fk_solver_info_proxy = rospy.ServiceProxy( service_prefix + side + '_arm_kinematics/get_fk_solver_info', GetKinematicSolverInfo) left_arm_solver_info = get_fk_solver_info_proxy() self.left_arm_joints = list() for joint in left_arm_solver_info.kinematic_solver_info.joint_names: self.left_arm_joints.append(joint) rospy.loginfo('Adding joint ' + str(joint)) self.request = GetPositionFKRequest() self.request.robot_state.joint_state = JointState() self.request.robot_state.joint_state.header.frame_id = side + base_frame self.request.robot_state.joint_state.name = left_arm_solver_info.kinematic_solver_info.joint_names self.request.robot_state.joint_state.position = [0] * len( self.request.robot_state.joint_state.name) #self.request.robot_state.joint_state.position[0] = 1.0 for joint in self.request.robot_state.joint_state.name: self.request.robot_state.joint_state.position[ self.request.robot_state.joint_state.name.index(joint)] = 0 rospy.Subscriber('joint_states', JointState, self.getJointState) self.request.header.frame_id = side + base_frame self.request.fk_link_names = list() # self.request.fk_link_names.append("left_shoulder_pan_link") # self.request.fk_link_names.append("left_shoulder_lift_link") # self.request.fk_link_names.append("left_arm_roll_link") # self.request.fk_link_names.append("left_elbow_link") # self.request.fk_link_names.append("left_forearm_link") # self.request.fk_link_names.append("left_wrist_link") self.request.fk_link_names.append("right_wrist_roll_link") while not rospy.is_shutdown(): self.request.header.stamp = rospy.Time.now() self.request.robot_state.joint_state.header.stamp = rospy.Time.now( ) #rospy.loginfo(self.request) try: response = get_fk_proxy(self.request) hand_link = response.pose_stamped[0] # q = list() # q.append(hand_link.pose.orientation.x) # q.append(hand_link.pose.orientation.y) # q.append(hand_link.pose.orientation.z) # q.append(hand_link.pose.orientation.w) #rpy = euler_from_quaternion(q) #rospy.loginfo(rpy) msg = PoseStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = side + base_frame msg.pose = hand_link.pose rospy.loginfo(response.pose_stamped) hand_pub.publish(msg) except rospy.ServiceException, e: print "Service call failed: %s" % e r.sleep()
def push_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name current_state = rospy.wait_for_message('l_arm_controller/state', FollowJointTrajectoryFeedback) start_angles = current_state.actual.positions full_state = rospy.wait_for_message('/joint_states', JointState) req = GetPositionFKRequest() req.header.frame_id = 'base_link' req.fk_link_names = [GRIPPER_LINK_FRAME] req.robot_state.joint_state = full_state if not self.set_planning_scene_diff_client(): rospy.logerr('%s: failed to set planning scene diff' % ACTION_NAME) self.action_server.set_aborted() return pose = self.get_fk_srv(req).pose_stamped[0].pose frame_id = 'base_link' approachpos = [pose.position.x, pose.position.y, pose.position.z] approachquat = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] push_distance = 0.40 grasppos = [ pose.position.x, pose.position.y - push_distance, pose.position.z ] graspquat = [0.00000, 0.00000, 0.70711, -0.70711] # attach object to gripper, they will move together obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.02 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [ collision_operation1, collision_operation2 ] res = self.check_cartesian_path_lists( approachpos, approachquat, grasppos, graspquat, start_angles, frame=frame_id, ordered_collision_operations=ordered_collision_operations) error_code_dict = { ArmNavigationErrorCodes.SUCCESS: 0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1, ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2, ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3, ArmNavigationErrorCodes.PLANNING_FAILED: 4 } trajectory_len = len(res.trajectory.joint_trajectory.points) trajectory = [ res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len) ] vels = [ res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len) ] times = [ res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len) ] error_codes = [ error_code_dict[error_code.val] for error_code in res.trajectory_error_codes ] # if even one code is not 0, abort if sum(error_codes) != 0: for ind in range(len(trajectory)): rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) rospy.loginfo("") for ind in range(len(trajectory)): rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind])) rospy.logerr('%s: attempted push failed' % ACTION_NAME) self.action_server.set_aborted() return req = FilterJointTrajectoryRequest() req.trajectory = res.trajectory.joint_trajectory req.trajectory.points = req.trajectory.points[ 1:] # skip zero velocity point req.allowed_time = rospy.Duration(2.0) filt_res = self.trajectory_filter_srv(req) goal = FollowJointTrajectoryGoal() goal.trajectory = filt_res.trajectory goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id) rospy.sleep(0.5) self.trajectory_controller.send_goal(goal) self.trajectory_controller.wait_for_result() state = self.trajectory_controller.get_state() if state == GoalStatus.SUCCEEDED: rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded( PushObjectResult(sound_msg.recorded_sound)) return rospy.logerr('%s: attempted push failed' % ACTION_NAME) self.stop_audio_recording_srv(False) self.action_server.set_aborted()