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)
