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