from moveit_ros_planning_interface import _moveit_move_group_interface
import rospy
import tf
rospy.init_node('plan_base')
tf_broadcaster = tf.TransformBroadcaster()
rospy.sleep(1)
def print_base_link(links):
print "Base coords: %s" % links['global_link'][0]
moveit = _moveit_move_group_interface.MoveGroup("base")
print "Starting"
print "Initial links:"
print_base_link(moveit.get_link_poses_compressed())
# This will compute a plan for [x,y,theta] on the floor space
moveit.set_joint_value_target([1.5, 0.0, 3.1415])
plan = moveit.compute_plan()
# print "Plan: %s" % plan
trans = plan['multi_dof_joint_trajectory']['points'][-1]['transforms'][0]
print "Transform: %s" % trans
# Now:
# 1- how do I get the updated poses given this plan? It's a multidof trajectory with frames
# as steps, not joint values
# 2- How do I update the initial pose with the end state in the robot_state, so that I ca
# plan from this new location?
def __init__(self, name, robot_description="robot_description"):
""" Specify the group name for which to construct this commander instance. Throws an exception if there is an initialization error. """
self._g = _moveit_move_group_interface.MoveGroup(name, robot_description)
from moveit_ros_planning_interface import _moveit_move_group_interface
moveit = _moveit_move_group_interface.MoveGroup("right_arm_and_torso")
print "Root link: "
print moveit.get_robot_root_link()
print "Root state: "
print moveit.get_link_poses_compressed()
