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