def service_left_fk(self, joint_state):
rospy.wait_for_service('/pr2_left_arm_kinematics/get_fk')
try:
## include <kinematics_msgs/GetKinematicSolverInfo.h> kinematics_msgs/GetPositionFK.h> moveit_msgs
moveit_fk = rospy.ServiceProxy('/pr2_left_arm_kinematics/get_fk',
GetPositionFK)
# A vector of link name for which forward kinematics must be computed
fk_link_names = ['l_wrist_roll_link']
joint_names = [
'l_upper_arm_roll_joint', 'l_shoulder_pan_joint',
'l_shoulder_lift_joint', 'l_forearm_roll_joint',
'l_elbow_flex_joint', 'l_wrist_flex_joint',
'l_wrist_roll_joint'
]
# joint_positions = []
# for i in range(7):
# joint_names.append('right_arm_j'+str(i)) # your names may vary
# joint_positions.append(0.8) # try some arbitrary joint angle
header = Header(0, rospy.Time.now(), "/base_link")
rs = moveit_commander.RobotState(
) # this is a moveir msg? http://docs.ros.org/api/moveit_msgs/html/msg/RobotState.html
rs.joint_state.name = joint_names
rs.joint_state.position = joint_state
rospy.logwarn('Requesting fk.')
fk_pose = moveit_fk(header, fk_link_names, rs)
print("FK LOOKUP:", fk_pose) # Lookup the pose
except rospy.ServiceException, e:
rospy.logerror("Service call failed: %s" % e)
def __init__(self, group_name, move_group='move_group'):
# wait for MoveIt! to come up
#client = actionlib.SimpleActionClient(move_group, moveit_msgs.msg.MoveGroupAction)
#rospy.loginfo("Waiting for '{0}' server".format(move_group))
#client.wait_for_server()
#rospy.loginfo("Found server '{0}'".format(move_group))
self.group_name = group_name
self.group = moveit_commander.MoveGroupCommander(self.group_name)
self.commander = moveit_commander.RobotCommander()
self.state = moveit_commander.RobotState()
self.copy = self.commander.get_joint_names(self.group_name)
self.config = self.group.get_current_joint_values()
#self.joint_names = self.copy[4:]
#self.configuration = self.config[2:]
#self.groupIK = 'manipulator2'
# service clients
rospy.loginfo("Waiting for 'compute_ik' service")
rospy.wait_for_service('/atrvjr/compute_ik')
self.ik_client = rospy.ServiceProxy('/atrvjr/compute_ik',
moveit_msgs.srv.GetPositionIK)
rospy.loginfo("Found service 'compute_ik'")
rospy.loginfo("Waiting for 'compute_fk' service")
rospy.wait_for_service('/atrvjr/compute_fk')
self.fk_client = rospy.ServiceProxy('/atrvjr/compute_fk',
moveit_msgs.srv.GetPositionFK)
rospy.loginfo("Found service 'compute_fk'")
def __init__(self, group_name, move_group='move_group'):
# wait for MoveIt! to come up
client = actionlib.SimpleActionClient(move_group,
moveit_msgs.msg.MoveGroupAction)
rospy.loginfo("Waiting for '{0}' server".format(move_group))
client.wait_for_server()
rospy.loginfo("Found server '{0}'".format(move_group))
self.group_name = group_name
self.group = moveit_commander.MoveGroupCommander(self.group_name)
self.commander = moveit_commander.RobotCommander()
self.state = moveit_commander.RobotState()
self.joint_names = self.commander.get_joint_names(self.group_name)
self.link_names = self.commander.get_link_names(self.group_name)
# service clients
rospy.loginfo("Waiting for 'compute_ik' service")
rospy.wait_for_service('/compute_ik')
self.ik_client = rospy.ServiceProxy('/compute_ik',
moveit_msgs.srv.GetPositionIK)
rospy.loginfo("Found service 'compute_ik'")
rospy.loginfo("Waiting for 'compute_fk' service")
rospy.wait_for_service('/compute_fk')
self.fk_client = rospy.ServiceProxy('/compute_fk',
moveit_msgs.srv.GetPositionFK)
rospy.loginfo("Found service 'compute_fk'")
def __init__(self, group_name):
self.group_name = group_name
self.commander = moveit_commander.RobotCommander()
self.state = moveit_commander.RobotState()
self.joint_names = self.commander.get_joint_names(self.group_name)
# service clients
rospy.loginfo('Waiting for "get_ik" service')
rospy.wait_for_service('get_ik')
self.ik_client = rospy.ServiceProxy('get_ik',
moveit_msgs.srv.GetPositionIK)
rospy.loginfo('Found service "get_ik"')
def __init__(self, group_name):
self.group_name = group_name
self.group = moveit_commander.MoveGroupCommander(self.group_name)
self.commander = moveit_commander.RobotCommander()
self.state = moveit_commander.RobotState()
self.joint_names = self.commander.get_joint_names(self.group_name)
# service clients
rospy.loginfo("Waiting for 'compute_ik' service")
rospy.wait_for_service('/compute_ik')
self.ik_client = rospy.ServiceProxy('/compute_ik',
moveit_msgs.srv.GetPositionIK)
rospy.loginfo("Found service 'compute_ik'")
plan = rospy.wait_for_message(
'/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory
) #ideally, rather than listen from this topic for the plan, we would just directly access it from the scene/group, but I am not sure how to do this. come back to later
jt = plan.trajectory[0].joint_trajectory #plan for the joint trajectory!
print("joint plan!")
#print(len(plan.trajectory))
#print(jt.points[0].positions[0]) #first index is which point, second index just gets joint value out of tuple
print(len(jt.points))
print("end joint plan!")
fk = rospy.ServiceProxy('/compute_fk', GetPositionFK)
ee_poses = []
for i in range(len(jt.points)): #loop through the plan
r = moveit_commander.RobotState()
header = Header(0, rospy.Time.now(), "/base_link")
joint_names = jt.joint_names
joint_positions = [jt.points[i].positions[0]]
fkIn = ["base_link", "door", "handle"]
r.joint_state.name = joint_names
r.joint_state.position = joint_positions
#rospy.loginfo(fk(header,fkIn,r))
#print("hey")
#print(r)
a = fk(header, fkIn, r)
#print(a.pose_stamped[-1]) #print fk pose, -1 because we only care about the fk of the last link, handle.
#print(a.pose_stamped)
ee_poses.append(a.pose_stamped[-1].pose)
print(ee_poses)