def fk_solve(self, joint_angles, link_names): """ Given a set of joint angles for the robot, use forward kinematics to determine the end-effector pose reached with those angles CRED: https://github.com/uts-magic-lab/moveit_python_tools/blob/master/src/moveit_python_tools/get_fk.py """ # Build joint state message joint_state = JointState() joint_state.header.frame_id = self.ref_frame joint_state.header.stamp = rospy.Time.now() joint_state.name = self.joint_names joint_state.position = joint_angles # Build the service request req = GetPositionFKRequest() req.header.frame_id = self.ref_frame req.fk_link_names = [link_names] req.robot_state.joint_state = joint_state # Try to send the request to the 'compute_fk' service try: resp = self.fk_srv.call(req) return resp except rospy.ServiceException: rospy.logerr("Service execption: " + str(rospy.ServiceException))
def get_left_arm_fk_pose(self, joints, frame_id): """ Calls the forward kinematics to get the pose of the wrist given the joints specified. :param joints: the joints configuration for the fk :param frame_id: which frame to return the wrist pose :return: (position, orientation) in frame_id """ req = GetPositionFKRequest() req.header.frame_id = frame_id req.fk_link_names = ['l_wrist_roll_link'] req.robot_state.joint_state.name = self.robot_state.left_joint_names req.robot_state.joint_state.position = joints res = self.left_fk(req) isinstance(res, GetPositionFKResponse) if res.error_code.val != 1: rospy.logerr("IK error, code is %d" % res.error_code.val) return None posestamped = res.pose_stamped[0] position = (posestamped.pose.position.x, posestamped.pose.position.y, posestamped.pose.position.z) orientation = (posestamped.pose.orientation.x, posestamped.pose.orientation.y, posestamped.pose.orientation.z, posestamped.pose.orientation.w) return position, orientation
def test(self, joint_values): print str(joint_values)+" ", fkr= GetPositionFKRequest() fkr.header.frame_id= self.group1.get_planning_frame() fkr.fk_link_names= [self.group1.get_end_effector_link()] fkr.robot_state= self.robot.get_current_state() fkr.robot_state.joint_state.position= list(fkr.robot_state.joint_state.position) for (j,v) in zip(self.group1.get_active_joints(), joint_values): fkr.robot_state.joint_state.position[fkr.robot_state.joint_state.name.index(j)]= v target= self.fk(fkr).pose_stamped[0] self.pub_state.publish( DisplayRobotState(state= fkr.robot_state) ) ik_seed= self.robot.get_current_state() ik_seed.joint_state.position= [0.0] * len(ik_seed.joint_state.position) ikr= GetPositionIKRequest() ikr.ik_request.group_name= self.group1.get_name() ikr.ik_request.robot_state= ik_seed ikr.ik_request.ik_link_name= self.group1.get_end_effector_link() ikr.ik_request.pose_stamped= target ikr.ik_request.timeout= rospy.Duration(rospy.get_param("~timeout", 6.0)) response= self.ik(ikr) if response.error_code.val == MoveItErrorCodes.SUCCESS: print "OK " else: print "FAILED "
def getFK(self, fk_link_names, joint_names, positions, frame_id='base_link'): """Get the forward kinematics of a joint configuration @fk_link_names list of string or string : list of links that we want to get the forward kinematics from @joint_names list of string : with the joint names to set a position to ask for the FK @positions list of double : with the position of the joints @frame_id string : the reference frame to be used""" gpfkr = GetPositionFKRequest() if type(fk_link_names) == type("string"): gpfkr.fk_link_names = [fk_link_names] else: gpfkr.fk_link_names = fk_link_names gpfkr.robot_state.joint_state.name = joint_names gpfkr.robot_state.joint_state.position = positions gpfkr.header.frame_id = frame_id # fk_result = GetPositionFKResponse() fk_result = self.fk_srv.call(gpfkr) return fk_result
def getFK(self, fk_link_names, joint_names, positions, frame_id='base_link'): """Get the forward kinematics of a joint configuration @fk_link_names list of string or string : list of links that we want to get the forward kinematics from @joint_names list of string : with the joint names to set a position to ask for the FK @positions list of double : with the position of the joints @frame_id string : the reference frame to be used""" gpfkr = GetPositionFKRequest() if type(fk_link_names) == type("string"): gpfkr.fk_link_names = [fk_link_names] else: gpfkr.fk_link_names = fk_link_names gpfkr.robot_state.joint_state.name = joint_names gpfkr.robot_state.joint_state.position = positions gpfkr.header.frame_id = frame_id #fk_result = GetPositionFKResponse() fk_result = self.fk_srv.call(gpfkr) return fk_result
def solve_fk(js): # TODO: test me rospy.wait_for_service('compute_fk') try: print("try to solve fk...") request = GetPositionFKRequest() request.fk_link_names = link_names request.header.frame_id = "/base_link" request.robot_state.joint_state = js fk = rospy.ServiceProxy('compute_fk', GetPositionFK) resp = fk(request) return parse_fk_resp(resp) except rospy.ServiceException as e: print('Service call failed - {}'.format(e))
def get_fk_service(self, left_angles, right_angles, links): msg = GetPositionFKRequest() msg.header.frame_id = 'base_link' msg.fk_link_names = links msg.robot_state.joint_state.name = self.left.JOINT_NAMES + self.right.JOINT_NAMES msg.robot_state.joint_state.position = left_angles + right_angles try: fk_service = rospy.ServiceProxy("compute_fk", GetPositionFK) resp = fk_service(msg) if resp.error_code.val != 1: print resp return -1 except rospy.ServiceException as e: rospy.logwarn("Exception on fk service {}".format(e)) return -1 return resp
def get_fk(self, joint_state, fk_links, frame_id): """ Get end-effector cartesian coordinate from joint coordinates; """ req = GetPositionFKRequest() req.header.frame_id = frame_id req.fk_link_names = fk_links req.robot_state.joint_state = joint_state try: resp = self.fk_srv.call(req) return resp except rospy.ServiceException as e: rospy.logerr("Service exception: " + str(e)) resp = GetPositionFKResponse() resp.error_code = 99999 # Failure return resp
def compute_f_k(self, compute_fk, joint_values): ''' compute the forward kinematics of the given joint values,the given joint values should be an array of (1,6),this function returns the fk response with the type of PoseStamped ''' fk_request = GetPositionFKRequest() links = self.move_group_link_names fk_request.fk_link_names = links state = RobotState() joint_names = self.active_joints state.joint_state.name = joint_names state.joint_state.position = joint_values fk_request.robot_state = state fk_response = compute_fk(fk_request) end_effector_pose = fk_response.pose_stamped[ len(fk_response.pose_stamped) - 1] return end_effector_pose
def compute_forward_kinematics(self,joint_values,goal_link): ''' compute the forward kinematics of the given joint values with reference to the reference link, return a posestamped ''' fk_request=GetPositionFKRequest() links=self.robot.get_link_names() fk_request.fk_link_names=links state=RobotState() joint_names=['wx_agv2_1','agv2_virx','agv2_viry','agv2_virz','joint_a1','joint_a2','joint_a3','joint_a4','joint_a5','joint_a6','joint_a7'] state.joint_state.name=joint_names state.joint_state.position=joint_values fk_request.robot_state=state fk_response=self.compute_fk(fk_request) index=fk_response.fk_link_names.index(goal_link) end_effector_pose=fk_response.pose_stamped[index] return end_effector_pose
def compute_f_k(self, compute_fk, joint_values): fk_request = GetPositionFKRequest() links = self.move_group_link_names fk_request.fk_link_names = links fk_request.header.frame_id = links[0] state = RobotState() joint_names = self.active_joints state.joint_state.name = joint_names state.joint_state.position = joint_values fk_request.robot_state = state fk_response = compute_fk(fk_request) #manipulator_first_link_pose=fk_response.pose_stamped[0] #print(manipulator_first_link_pose) end_effector_pose = fk_response.pose_stamped[ len(fk_response.pose_stamped) - 1] #print(end_effector_pose) return end_effector_pose
def compute_fk(self, joint_state): """Computes forward kinematics for the given joint state. Args: joint_state: sensor_msgs/JointState. Returns: A geometry_msgs/PoseStamped of the wrist position, False otherwise. """ request = GetPositionFKRequest() request.header.frame_id = 'base_link' request.fk_link_names = ['wrist_roll_link'] request.robot_state.joint_state = joint_state response = self._compute_fk(request) error_str = moveit_error_string(response.error_code.val) success = error_str == 'SUCCESS' if not success: return False return response.pose_stamped
def _get_fk_ros(self, frame_id = None, state=None): rqst = GetPositionFKRequest() rqst.header.frame_id = self._world if frame_id is None else frame_id rqst.fk_link_names = [self.endpoint_name()] if isinstance(state, RobotState): rqst.robot_state = state elif isinstance(state, JointState): rqst.robot_state.joint_state = state elif state is None: rqst.robot_state = self.get_current_state() else: raise AttributeError("Provided state is an invalid type") fk_answer = self._kinematics_services['fk']['ros']['service'].call(rqst) if fk_answer.error_code.val==1: return fk_answer.pose_stamped[0] else: return None
def _get_fk_ros(self, frame_id=None, state=None): rqst = GetPositionFKRequest() rqst.header.frame_id = self._world if frame_id is None else frame_id rqst.fk_link_names = [self.endpoint_name()] if isinstance(state, RobotState): rqst.robot_state = state elif isinstance(state, JointState): rqst.robot_state.joint_state = state elif state is None: rqst.robot_state = self.get_current_state() else: raise AttributeError("Provided state is an invalid type") fk_answer = self._kinematics_services['fk']['ros']['service'].call( rqst) if fk_answer.error_code.val == 1: return fk_answer.pose_stamped[0] else: return None
def get_fk_pose(self, joint_states, fk_links, frame_id): if self.use_moveit: req = GetPositionFKRequest() req.header.frame_id = frame_id req.fk_link_names = [fk_links] req.robot_state.joint_state = joint_states try: resp = self.compute_fk.call(req) return resp.pose_stamped[0] except rospy.ServiceException as e: rospy.logerr("Service exception: " + str(e)) resp = GetPositionFKResponse() resp.error_code = 99999 # Failure return resp else: #Implement non-moveit FK pass
def get_fk(self, joint_state, fk_link=None, frame_id=None): """ Do an FK call to with. :param sensor_msgs/JointState joint_state: JointState message containing the full state of the robot. :param str or None fk_link: link to compute the forward kinematics for. """ if fk_link is None: fk_link = self.fk_link req = GetPositionFKRequest() req.header.frame_id = 'base_link' req.fk_link_names = [self.fk_link] req.robot_state.joint_state = joint_state try: resp = self.fk_srv.call(req) return resp except rospy.ServiceException as e: rospy.logerr("Service exception: " + str(e)) resp = GetPositionFKResponse() resp.error_code = 99999 # Failure return resp
def get_position_fk(robot_state, fk_link_names, persistent=False, wait_timeout=None): """Call compute forward kinematics service INPUT robot_state [RobotState message] fk_link_names [List of Strings] persistent [Bool, default=False] wait_timeout [Float, default=None] OUTPUT response [GetPositionFKResponse] """ srv_proxy = get_service_proxy(SRV_GET_POSITION_FK, GetPositionFK, persistent, wait_timeout) req = GetPositionFKRequest() req.robot_state = robot_state req.fk_link_names = fk_link_names res = srv_proxy(req) return res
def call(self, positions, joint_names=['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6'], links=["right_gripper_tip"], frame_id="/base"): """ Call the forward kinematics service "/compute_fk" to get FK of a joint configuration. Parameters ---------- links : list list of links that we want to get the forward kinematics from. joint_names : list List of strings with the joint names. positions : list List of doubles representing the the position of the joints. frame_id : string Reference frame. Returns ------- : ForwardKinematicsResponse The ForwardKinematicsResponse response built from from the /compute_fk service response. """ request = GetPositionFKRequest() request.fk_link_names = links request.robot_state.joint_state.name = joint_names request.robot_state.joint_state.position = positions request.header.frame_id = frame_id response = self.service.call(request) # check if there is a moveit failure. if response.error_code == 99999: return ForwardKinematicsResponse(False, None) else: return ForwardKinematicsResponse(True, response.pose_stamped[0].pose)
from moveit_msgs.msg import MoveItErrorCodes if __name__ == '__main__': try: rospy.init_node("fk_node") robot = RobotCommander() group_name = "manipulator" group = MoveGroupCommander(group_name) connection = rospy.ServiceProxy("/compute_fk", GetPositionFK) rospy.loginfo("waiting for fk server") connection.wait_for_service() rospy.loginfo("server found") request = GetPositionFKRequest() request.fk_link_names = robot.get_link_names(group_name) request.header.frame_id = robot.get_root_link() request.robot_state.joint_state.name = group.get_joints()[0:-1] # get joint state joint_states= raw_input("enter joint state j1 j2 j3 j4 j5 j6 j7 \n") joints= [float(idx) for idx in joint_states.split(' ')] request.robot_state.joint_state.position = joints response = connection(request) if response.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("solution found") rospy.loginfo(response.pose_stamped[-1]) else:
from copy import copy collision_srv = rospy.ServiceProxy('/check_state_validity', GetStateValidity) collision_srv.wait_for_service() collision_req = GetStateValidityRequest() collision_req.robot_state.joint_state.name = [ "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" ] fk_srv = rospy.ServiceProxy('/compute_fk', GetPositionFK) fk_srv.wait_for_service() fk_req = GetPositionFKRequest() fk_req.robot_state.joint_state.name = [ "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" ] fk_req.fk_link_names = ["tool0"] fk_req.header.frame_id = "base_link" marker_pub = rospy.Publisher('visualiation_markers', MarkerArray, queue_size=100) def in_collision(state): collision_req.robot_state.joint_state.header.stamp = rospy.Time.now() collision_req.robot_state.joint_state.position = state res = collision_srv.call(collision_req) return not res.valid def get_fk(state):