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 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): #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 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 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 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()