Sending a json goal

Giskard as a generic json interface, that can be used to call any constraint defined constraints.py, without having to modify the ros messages. In this example we are calling the Pointing constraint over the json interface. the Giskard wrapper offers the method set_json_goal, it is used almost as if you would call the __init__ of Pointing. Pointings __init__ method has the following header.

def __init__(self, god_map, tip_link, goal_point, root_link=None, pointing_axis=None, weight=WEIGHT_BELOW_CA,
             goal_constraint=True):
    """
    Uses the kinematic chain from root_link to tip_link to move the pointing axis, such that it points to the goal point.
    :param tip_link: str, name of the tip of the kin chain
    :param goal_point: PointStamped as json, where the pointing_axis will point towards
    :param root_link: str, name of the root of the kin chain
    :param pointing_axis: Vector3Stamped as json, default is z axis, this axis will point towards the goal_point
    :param weight: float, default WEIGHT_BELOW_CA
    """

The parameters self and god_map always have to be ignored. Parameters without a default value have to be set and those with a default are optional. The weight parameter exists for every constraint as well, but should be ignored most of the time. Now follows a small script that calls a goal with only this constraint.

import rospy
from geometry_msgs.msg import Vector3Stamped
from rospy_message_converter.message_converter import convert_ros_message_to_dictionary
 
from giskardpy.python_interface import GiskardWrapper
from giskardpy.tfwrapper import lookup_point
 
rospy.init_node('test', anonymous=True)
giskard = GiskardWrapper()
 
goal_point = lookup_point('map', 'r_gripper_tool_frame')
tip = 'head_mount_kinect_rgb_link'
root = 'torso_lift_link'
pointing_axis = Vector3Stamped()
pointing_axis.header.frame_id = tip
pointing_axis.vector.x = 1
 
giskard.set_json_goal('Pointing',
                      tip_link=tip,
                      root_link=root,
                      goal_point=goal_point,
                      pointing_axis=pointing_axis)
giskard.plan_and_execute()

It is important that the keywords are used when setting the parameters. Internally, set_json_goal turns the parameters in to a json string with the key value pairs.

{
  "root": "torso_lift_link", 
  "pointing_axis": {
    "header": {
      "stamp": {
        "secs": 0, 
        "nsecs": 0
      }, 
      "frame_id": "head_mount_kinect_rgb_link", 
      "seq": 0
    }, 
    "vector": {
      "y": 0.0, 
      "x": 1, 
      "z": 0.0
    }
  }, 
  "tip": "head_mount_kinect_rgb_link", 
  "goal_point": {
    "header": {
      "stamp": {
        "secs": 0, 
        "nsecs": 0
      }, 
      "frame_id": "map", 
      "seq": 0
    }, 
    "point": {
      "y": -0.188, 
      "x": 0.4795782004026931, 
      "z": 1.3782186566800914
    }
  }
}

This interface can also be used to call the standard cartesian and joint goals with additional parameters, like max_speed.