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 __init__(self): ik_serv_name = '/compute_ik' fk_serv_name = '/compute_fk' self.frame_id = "torso_lift_link" self.r_link_name = 'r_wrist_roll_link' self.l_link_name = 'l_wrist_roll_link' self.r_group_name = 'right_arm' self.l_group_name = 'left_arm' #Set up right/left FK service connections print 'Waiting for forward kinematics services...' rospy.wait_for_service(fk_serv_name) self.getPosFK = rospy.ServiceProxy(fk_serv_name, GetPositionFK) self.getPosFKPersistent = rospy.ServiceProxy(fk_serv_name, GetPositionFK, persistent=True) print "OK" #Set up right/left FK service requests self.FKreq = [GetPositionFKRequest(), GetPositionFKRequest()] self.FKreq[0].header.frame_id = self.frame_id self.FKreq[0].fk_link_names = [self.r_link_name] self.FKreq[0].robot_state.joint_state.name = self.getJointNames(0) self.FKreq[1].header.frame_id = self.frame_id self.FKreq[1].fk_link_names = [self.l_link_name] self.FKreq[1].robot_state.joint_state.name = self.getJointNames(1) #Set up right/left IK service connections print 'Waiting for inverse kinematics services...' rospy.wait_for_service(ik_serv_name) self.getPosIK = rospy.ServiceProxy(ik_serv_name, GetPositionIK) self.getPosIKPersistent = rospy.ServiceProxy(ik_serv_name, GetPositionIK, persistent=True) print 'OK' #Set up right/left IK service requests self.IKreq = [GetPositionIKRequest(), GetPositionIKRequest()] self.IKreq[0].ik_request.robot_state.joint_state.position = [0] * 7 self.IKreq[ 0].ik_request.robot_state.joint_state.name = self.getJointNames(0) self.IKreq[0].ik_request.group_name = self.r_group_name self.IKreq[0].ik_request.ik_link_name = self.r_link_name self.IKreq[0].ik_request.pose_stamped.header.frame_id = self.frame_id self.IKreq[1].ik_request.robot_state.joint_state.position = [0] * 7 self.IKreq[ 1].ik_request.robot_state.joint_state.name = self.getJointNames(1) self.IKreq[1].ik_request.group_name = self.l_group_name self.IKreq[1].ik_request.ik_link_name = self.l_link_name self.IKreq[1].ik_request.pose_stamped.header.frame_id = self.frame_id
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 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 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 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 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 _get_coordinates(self, point, arm): if arm not in ["right_arm", "left_arm"]: #can only draw points for pr2 arms return None FK_NAME = "/compute_fk" FK_INFO_NAME = "/pr2_%s_kinematics/get_fk_solver_info" % (arm) info_client = rospy.ServiceProxy(FK_INFO_NAME, GetKinematicSolverInfo) info_request = GetKinematicSolverInfoRequest() rospy.wait_for_service(FK_INFO_NAME) info_response = info_client(info_request) fk_client = rospy.ServiceProxy(FK_NAME, GetPositionFK) fk_request = GetPositionFKRequest() fk_request.header.frame_id = "odom_combined" fk_request.fk_link_names.append("%s_wrist_roll_link" % (arm[0])) fk_request.robot_state.joint_state.name = info_response.kinematic_solver_info.joint_names #fk_request.robot_state = self._get_robot_state() fk_request.robot_state.joint_state.position = [] for i in xrange(len(info_response.kinematic_solver_info.joint_names)): fk_request.robot_state.joint_state.position.append(point[i]) rospy.wait_for_service(FK_NAME) fk_solve_response = fk_client(fk_request) if (fk_solve_response.error_code.val == fk_solve_response.error_code.SUCCESS): position = fk_solve_response.pose_stamped[0].pose.position return (position.x, position.y, position.z) else: rospy.loginfo("Forward kinematics service call failed") return None
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 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 _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 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_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 __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('testkin/GetKinematicSolverInfo') 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 = GetPositionFKRequest() self.request.fk_link_names = self.links self.request.robot_state.joint_state = JointState() self.request.robot_state.joint_state.header.frame_id = 'base_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] = 0.0 self.request.robot_state.joint_state.position[1] = 0.0 self.request.robot_state.joint_state.position[2] = 0.0 self.request.robot_state.joint_state.position[3] = 0.0 self.request.robot_state.joint_state.position[4] = 0.0 self.request.robot_state.joint_state.position[5] = 0.0 self.request.header.frame_id = "base_link" while not rospy.is_shutdown(): try: response = get_fk_proxy(self.request) print "Service success!!!" rospy.loginfo(response) except rospy.ServiceException, e: print "Service call failed: %s" % e r.sleep()
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)
def FK(self,jointvals): self.request = GetPositionFKRequest() self.request.fk_link_names = self.linknames self.request.robot_state.joint_state = self.joint_state # JointState() self.request.robot_state.joint_state.header.frame_id = 'base_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] = jointvals.position[0] # self.request.robot_state.joint_state.position[1] = jointvals.position[1] # self.request.robot_state.joint_state.position[2] = jointvals.position[2] # self.request.robot_state.joint_state.position[3] = jointvals.position[3] # self.request.robot_state.joint_state.position[4] = jointvals.position[4] # self.request.robot_state.joint_state.position[5] = jointvals.position[5] self.request.header.frame_id = "base_link" try: response = self.get_fk_proxy(self.request) #rospy.loginfo(response) return response except rospy.ServiceException, e: print "Service call failed: %s" % e self.FK(jointvals)
from tf.transformations import quaternion_from_euler 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])
def on_enter(self, userdata): self._planning_failed = False self._control_failed = False self._success = False self._joint_names = userdata.joint_names self._initial_state = RobotState() self._initial_state.joint_state.position = copy.deepcopy( userdata.joint_values) self._initial_state.joint_state.name = copy.deepcopy(self._joint_names) #print self._initial_state.joint_state.name #print self._initial_state.joint_state.position self._srv_req = GetPositionFKRequest() self._srv_req.robot_state = self._initial_state self._srv_req.header.stamp = rospy.Time.now() self._srv_req.header.frame_id = "world" self._srv_req.fk_link_names = [self._tool_link] try: srv_result = self._fk_srv.call(self._fk_srv_topic, self._srv_req) self._failed = False except Exception as e: Logger.logwarn('Could not call FK: ' + str(e)) self._planning_failed = True return grasp_pose = srv_result.pose_stamped[0].pose grasp_pose_stamped = srv_result.pose_stamped[0] # Create a pre-grasp approach pose with an offset of 0.3 pre_grasp_approach_pose = copy.deepcopy(grasp_pose_stamped) pre_grasp_approach_pose.pose.position.z += self._offset + 0.3 # Create an object to MoveGroupInterface for the current robot. self._mgi_active_robot = MoveGroupInterface( self._current_group_name, self._current_group_name + '_base_link') # TODO: clean up in on_exit self._mgi_active_robot.moveToPose(pre_grasp_approach_pose, self._tool_link) # Use cartesian motions to pick the object. cartesian_service_req = GetCartesianPathRequest() cartesian_service_req.start_state.joint_state = rospy.wait_for_message( self._current_group_name + '/joint_states', sensor_msgs.msg.JointState) cartesian_service_req.header.stamp = rospy.Time.now() cartesian_service_req.header.frame_id = "world" cartesian_service_req.link_name = self._tool_link cartesian_service_req.group_name = self._current_group_name cartesian_service_req.max_step = 0.01 cartesian_service_req.jump_threshold = 0 cartesian_service_req.avoid_collisions = True grasp_pose.position.z += self._offset + 0.16 # this is basically the toolframe (with the box as the tool) cartesian_service_req.waypoints.append(grasp_pose) try: cartesian_srv_result = self._cartesian_srv.call( self._cartesian_srv_topic, cartesian_service_req) self._failed = False except Exception as e: Logger.logwarn('Could not call Cartesian: ' + str(e)) self._planning_failed = True return if cartesian_srv_result.fraction < 1.0: Logger.logwarn('Cartesian failed. fraction: ' + str(cartesian_srv_result.fraction)) self._planning_failed = True return traj_goal = moveit_msgs.msg.ExecuteTrajectoryGoal() traj_goal.trajectory = cartesian_srv_result.solution self._traj_client.send_goal(traj_goal) self._traj_client.wait_for_result() self._traj_exec_result = self._traj_client.get_result()
from geometry_msgs.msg import PointStamped from visualization_msgs.msg import Marker, MarkerArray import rospy import time 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)