Combining Goals

This example shows how you can combine joint and cartesian goals. It can be done similarly for all types of goals. Just have to take into account that it can be difficult to work with conflicting goals. It can result in unstable or unexpected behavior. The goal might also not be solvable at all.

This script creates a joint goal that conflicts with a cartesian goal.

import rospy
from giskardpy.python_interface import GiskardWrapper
from geometry_msgs.msg import PoseStamped
p = PoseStamped()
p.header.frame_id = u'map'
p.header.stamp = rospy.get_rostime()
p.pose.position.x = 0.6
p.pose.position.y = 0.2
p.pose.position.z = 1.0
p.pose.orientation.w = 1
# create a GiskardWrapper object
giskard_wrapper = GiskardWrapper()
# add a cartesian goal via the GiskardWrapper
giskard_wrapper.set_cart_goal('base_link', 'l_gripper_tool_frame', p)
# add a joint goal via the GiskardWrapper
giskard_wrapper.set_joint_goal({'torso_lift_joint': 0.3})
# execute all added goals

The left hand is supposed to go to a specific pose, but because base_link is the root, 'torso_lift_joint' is part of the chain. So Giskard would usually use that joint to achieve the cartesian goal. But we also command torso_lift_joint to go to 0.3, this creates a conflict with the cartesian goal. Giskard will now try, and in this case succeed, to satisfy both goals.