How to implement new goals

Implementing a new Goal

Create a new file

Creating a new goal is similar to creating a new monitor. You either have to create a new file in giskardpy/src/giskardpy/goals, or you have to link your own package in your Giskard config.

The bare minimum for a new class looks like this:

from giskardpy import casadi_wrapper as cas
from giskardpy.goals.goal import Goal


class MyGoal(Goal):
    def __init__(self, name: str, 
                 # add custom parameters
                 start_condition: cas.Expression = cas.TrueSymbol,
                 hold_condition: cas.Expression = cas.FalseSymbol, 
                 end_condition: cas.Expression = cas.TrueSymbol):
        super().__init__(name=name,
                         start_condition=start_condition,
                         hold_condition=hold_condition,
                         end_condition=end_condition)
        # process __init__ parameters
        # create tasks
        # link tasks to monitors

As with monitors, you can add arbitrary additional __init__ parameters, as long as they are of a primitive type or a ROS message. Now you have two options to implement the behavior, all of which can also be combined:

1. A completely new goal.
2. Goal as a combination of existing goals.

A completely new Goal

Every goal is a set of tasks and monitors. A monitor is an object which observes something and decides on a bool state.
They are used to define start_condition of other monitors or tasks and for hold and `end_conditions of tasks. A task is a set of constraints with the same start, hold and end_condition. In a nutshell, constraints cause motions by constraining the joint velocity of a robot. For example, if the goal is a positive joint position, the velocity is constrained to a positive value until the goal is reached.
Let's look at the Cartesian position goal as an example:

class CartesianPosition(Goal):
    default_reference_velocity = 0.2

    def __init__(self, root_link: str, tip_link: str, goal_point: PointStamped,
                 root_group: Optional[str] = None,
                 tip_group: Optional[str] = None,
                 reference_velocity: Optional[float] = None,
                 weight: float = WEIGHT_ABOVE_CA,
                 absolute: bool = False,
                 name: Optional[str] = None,
                 start_condition: cas.Expression = cas.TrueSymbol,
                 hold_condition: cas.Expression = cas.FalseSymbol,
                 end_condition: cas.Expression = cas.TrueSymbol):
        """
        See CartesianPose.
        """
        self.root_link = god_map.world.search_for_link_name(root_link, root_group)
        self.tip_link = god_map.world.search_for_link_name(tip_link, tip_group)
        if name is None:
            name = f'{self.__class__.__name__}/{self.root_link}/{self.tip_link}'
        super().__init__(name)
        if reference_velocity is None:
            reference_velocity = self.default_reference_velocity
        self.reference_velocity = reference_velocity
        self.weight = weight
        if absolute:
            root_P_goal = transform_msg_and_turn_to_expr(self.root_link, goal_point, cas.TrueSymbol)
        else:
            root_P_goal = transform_msg_and_turn_to_expr(self.root_link, goal_point, start_condition)
        r_P_c = god_map.world.compose_fk_expression(self.root_link, self.tip_link).to_position()
        task = self.create_and_add_task()
        task.add_point_goal_constraints(frame_P_goal=root_P_goal,
                                        frame_P_current=r_P_c,
                                        reference_velocity=self.reference_velocity,
                                        weight=self.weight)
        self.connect_monitors_to_all_tasks(start_condition=start_condition, hold_condition=hold_condition,
                                           end_condition=end_condition)

Its __init__ has several additional parameters that are needed to define a position goal. For example, the root and tip link of the kinematic chain and a goal point. Next, have to create a task and add constraints to it. Tasks offer several options to add constraints.
In this case we want to use add_point_goal_constraints. It requires a symbolic expressions that describes our goal point and one that describes our current position. Both have to be expressed relative to the same frame. The current expression must have a dependency on the joint symbols of the robot. Here we achieve that by using god_map.world to compose a forward kinematics expression. This is the most common way to introduce joint symbol dependence for goals in Cartesian space.
The goal point is turned into an expression using transform_msg_and_turn_to_expr. This function transforms the point into the root link and turns it into an expression. In addition, we can link it to a condition and Giskard will recompute the goal when that condition turns True. This is useful to describe relative goals in a chain of Cartesian goals.
The reference velocity is a soft limit for the velocity of this constraint, it is used to normalize the constraints in respect to goals with different units, e.g. rotation goals vs. translation goals.
Finally, we link all of our tasks to the conditions of this motion goal using self.connect_monitors_to_all_tasks(). This function should get called at the end of all "simple" goals, those that don't define their own monitors.

A Goal as a Combination of existing Goals

The simplest way to implement a new goal is to define it as a composition of already defined goals. The goal for a Cartesian pose is implemented like that:

class CartesianPose(Goal):
    def __init__(self,
                 root_link: str,
                 tip_link: str,
                 goal_pose: PoseStamped,
                 root_group: Optional[str] = None,
                 tip_group: Optional[str] = None,
                 reference_linear_velocity: Optional[float] = None,
                 reference_angular_velocity: Optional[float] = None,
                 name: Optional[str] = None,
                 absolute: bool = False,
                 weight=WEIGHT_ABOVE_CA,
                 start_condition: cas.Expression = cas.TrueSymbol,
                 hold_condition: cas.Expression = cas.FalseSymbol,
                 end_condition: cas.Expression = cas.TrueSymbol):
        """
        This goal will use the kinematic chain between root and tip link to move tip link into the goal pose.
        The max velocities enforce a strict limit, but require a lot of additional constraints, thus making the
        system noticeably slower.
        The reference velocities don't enforce a strict limit, but also don't require any additional constraints.
        :param root_link: name of the root link of the kin chain
        :param tip_link: name of the tip link of the kin chain
        :param goal_pose: the goal pose
        :param root_group: a group name, where to search for root_link, only required to avoid name conflicts
        :param tip_group: a group name, where to search for tip_link, only required to avoid name conflicts
        :param absolute: if False, the goal is updated when start_condition turns True.
        :param reference_linear_velocity: m/s
        :param reference_angular_velocity: rad/s
        :param weight: default WEIGHT_ABOVE_CA
        """
        self.root_link = god_map.world.search_for_link_name(root_link, root_group)
        self.tip_link = god_map.world.search_for_link_name(tip_link, tip_group)
        if name is None:
            name = f'{self.__class__.__name__}/{self.root_link}/{self.tip_link}'
        super().__init__(name)
        position_name = f'{self.name}/position'
        orientation_name = f'{self.name}/orientation'
        if reference_linear_velocity is None:
            reference_linear_velocity = CartesianPosition.default_reference_velocity
        if reference_angular_velocity is None:
            reference_angular_velocity = CartesianOrientation.default_reference_velocity
        self.weight = weight

        goal_point, goal_quaternion = split_pose_stamped(goal_pose)

        self.add_constraints_of_goal(CartesianPosition(root_link=root_link,
                                                       tip_link=tip_link,
                                                       goal_point=goal_point,
                                                       root_group=root_group,
                                                       tip_group=tip_group,
                                                       reference_velocity=reference_linear_velocity,
                                                       weight=self.weight,
                                                       name=position_name,
                                                       absolute=absolute,
                                                       start_condition=start_condition,
                                                       hold_condition=hold_condition,
                                                       end_condition=end_condition))

        self.add_constraints_of_goal(CartesianOrientation(root_link=root_link,
                                                          tip_link=tip_link,
                                                          goal_orientation=goal_quaternion,
                                                          root_group=root_group,
                                                          tip_group=tip_group,
                                                          reference_velocity=reference_angular_velocity,
                                                          weight=self.weight,
                                                          name=orientation_name,
                                                          absolute=absolute,
                                                          start_condition=start_condition,
                                                          hold_condition=hold_condition,
                                                          end_condition=end_condition))

A Cartesian pose goal is a composition of a Cartesian position and a Cartesian orientation goal. After processing the __init__ parameters, it creates instances for other motion goal and hands them over to self.add_constraints_of_goal calls.