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()