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 rospy.init_node(u'test') 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 giskard_wrapper.plan_and_execute()
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.