Sending Cartesian Goals
The GiskardWrapper class has a method called set_cart_goal(root_link, tip_link, position)
to set cartesian goals. The first argument specifies the root and the second argument the tip of the kinematic chain that will be used to solve this goal. The third argument is a PoseStamped
that describes the goal position of the tip link. Below is a small code example, where a goal is set for both hands at the same time.
import rospy from giskardpy.python_interface import GiskardWrapper from geometry_msgs.msg import PoseStamped, Point, Quaternion # init ros node rospy.init_node(u'test') giskard_wrapper = GiskardWrapper() # Create a goal for the right hand root = u'base_link' r_goal = PoseStamped() r_goal.header.frame_id = 'r_gripper_tool_frame' r_goal.header.stamp = rospy.get_rostime() r_goal.pose.position = Point(-0.1, 0, 0) r_goal.pose.orientation = Quaternion(0, 0, 0, 1) giskard_wrapper.set_cart_goal(root, 'r_gripper_tool_frame', r_goal) # Create a goal for the left hand l_goal = PoseStamped() l_goal.header.frame_id = 'l_gripper_tool_frame' l_goal.header.stamp = rospy.get_rostime() l_goal.pose.position = Point(-0.05, 0, 0) l_goal.pose.orientation = Quaternion(0, 0, 0, 1) giskard_wrapper.set_cart_goal(root, 'l_gripper_tool_frame', l_goal) giskard_wrapper.plan_and_execute()