def get_group_state_values(group_state_name):
""" Function to parse the SRDF to obtain the
joint configuration and names for a given group state
Parameters
----------
group_state_name : str
valid group state name from the SRDF for the given group
Returns
-------
tuple
Returns a tuple of two lists - one containing the joint
configuration values and the other containing the joint names
"""
robot = SRDF.from_parameter_server()
joint_values = [
joint.value[0]
for joint in robot.group_state_map[group_state_name].joints
]
joint_names = [
joint.name for joint in robot.group_state_map[group_state_name].joints
]
rospy.loginfo("Joint names {}".format(joint_names))
rospy.loginfo("Joint values {}".format(joint_values))
return joint_values, joint_names
def __init__(self, context):
super(MoveitCommanderWidget, self).__init__()
self.setObjectName('MoveitCommanderWidget')
self.robot = SRDF.from_parameter_server()
self.groups = self.get_groups()
self.commanders = [
moveit_commander.MoveGroupCommander(group) for group in self.groups
]
# list of QLineEdit fields used to display joint values
self.text_joint_values = []
self.MAX_COLUMNS = 4
self.tab_widget = QTabWidget()
for g in self.groups:
frame = self.setup_group_frame(g)
self.tab_widget.addTab(frame, g)
widget_layout = QVBoxLayout()
widget_layout.addWidget(self.tab_widget)
self.setLayout(widget_layout)
group_state_joint_angles = []
for x in range(6):
group_state_joint_angles.append(
float(
str(robot.group_states[statedict[state_name]].joints[x].value)
[1:-1]))
return group_state_joint_angles
if __name__ == "__main__":
# Create a node
rospy.init_node("mobmanip")
#Parse the SRDF from the parameter server
robot = SRDF.from_parameter_server()
# start moveit part
group = moveit_commander.MoveGroupCommander("manipulator")
group.set_planner_id("RRTConnectkConfigDefault")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# Make sure sim time is working
while not rospy.Time.now():
pass
# Setup clients
move_base = MoveBaseClient()