Write your own Constraints

To write your own constraints you need to:

  1. Create a new file in src/goals.
  2. Create new Class which inherits from Goal (defined in src/goals/goal.py).
  3. Implement __init__
  4. Implement make_constraints
  5. Implement __str__
  6. Call your constraint via the json interface

We will use the Pointing constraint as an example.

class Pointing(Goal):
    def __init__(self, tip_link, goal_point, root_link, pointing_axis=None, max_velocity=0.3,
                 weight=WEIGHT_BELOW_CA, **kwargs):
        """
        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
        """
        super(Pointing, self).__init__(**kwargs)
        self.weight = weight
        self.max_velocity = max_velocity
        self.root = root_link
        self.tip = tip_link
        self.root_P_goal_point = tf.transform_point(self.root, goal_point)
 
        if pointing_axis is not None:
            self.tip_V_pointing_axis = tf.transform_vector(self.tip, pointing_axis)
            self.tip_V_pointing_axis.vector = tf.normalize(self.tip_V_pointing_axis.vector)
        else:
            self.tip_V_pointing_axis = Vector3Stamped()
            self.tip_V_pointing_axis.header.frame_id = self.tip
            self.tip_V_pointing_axis.vector.z = 1
 
    def make_constraints(self):
        root_T_tip = self.get_fk(self.root, self.tip)
        root_P_goal_point = self.get_parameter_as_symbolic_expression(u'root_P_goal_point')
        tip_V_pointing_axis = self.get_parameter_as_symbolic_expression(u'tip_V_pointing_axis')
 
        root_V_goal_axis = root_P_goal_point - w.position_of(root_T_tip)
        root_V_goal_axis /= w.norm(root_V_goal_axis)  # FIXME avoid /0
        root_V_pointing_axis = w.dot(root_T_tip, tip_V_pointing_axis)
 
        self.add_vector_goal_constraints(frame_V_current=root_V_pointing_axis,
                                         frame_V_goal=root_V_goal_axis,
                                         reference_velocity=self.max_velocity,
                                         weight=self.weight)
 
    def __str__(self):
        s = super(Pointing, self).__str__()
        return u'{}/{}/{}'.format(s, self.root, self.tip)

__init__

The __init__ function must start like this:

def __init__(self, **kwargs):
    super(Pointing, self).__init__(**kwargs)

You can now define any parameters you may need:

    def __init__(self, tip_link, goal_point, root_link, pointing_axis=None, max_velocity=0.3,
                 weight=WEIGHT_BELOW_CA, **kwargs):
        super(Pointing, self).__init__(**kwargs)

In our case, these are two links which define a kinematic chain, a point, towards which the tip will point and an axis that will be used for the pointing. You parameters can only be of type str, float or any ROS message! In the remainder of the init, you can do some preprocessing, like transforming messages into different frames. But no casadi code! Make sure to save all parameters in a class attribute.

You can also add constraints of other goals at this point, for example:

        self.add_constraints_of_goal(CartesianPosition(root_link=root_link,
                                                       tip_link=tip_link,
                                                       goal=goal,
                                                       max_velocity=max_linear_velocity,
                                                       weight=weight,
                                                       **kwargs))

Again, no casadi code here. Call the classes as if through the json interface.

__str__

This function needs to make sure that a str(self) call creates a unique string, in case multiple constraints of this type are added at the same time. This usually means that you use your __init__ parameters to create a unique string, e.g.

    def __str__(self):
        s = super(Pointing, self).__str__()
        return u'{}/{}/{}'.format(s, self.root, self.tip)

make_constraints

This function consists of three parts.

  1. Create symbol references for Godmap entries.
  2. Write the constraints.
  3. Add the constraints.
Step 1

There are two ways to get Symbols.

  1. godmap.to_expr, which creates a symbol for a given path. Make sure to only refer to floats.
  2. self.get_parameter_as_symbolic_expression is a wrapper around the previous option for convenience. Just pass the name of a class attribute and get a symbol back.

Both will automaticall turn ROS message into usable datatypes, if it is implemented. For example, if I request the symbol for a PointStamped, it will automatically return a casadi matrix of length for with 1 as its last enty.

Important: This function is only executed once, during the initialization. Afterwards, only the expressions get evaluated with the latest data from the Godmap.

Step 2

During step 2, you have to use Casadi to create a symbolic expressions for your constraint. There exists a wrapper around casadi, which you should use. The wrapper provides a numpy like interface. Keep in mind that you are writing a symbolic expression, though. Nothing is evaluated at this point! If you are using standard python like ifs or loops you are most likely doing it wrong. If you want to use ifs, there are some option defined in the wrapper. Loops as part of a symbolic expressions are not possible.

To define a constraint you need:

  1. expression (e(o))
  2. upper (u_A)
  3. lower (l_A)
  4. weight
  5. reference_velocity (used for weight normalization, just hardcode something reasonable)

In short a constraint will try to keep the rate of change of expression between upper and lower. Weight describes how expansive it is to violate these limits.

All of these 5 must be casadi expressions or floats. But it doesn't make sense for expression to be a float.

To estimate the rate of change of expression, Giskard will compute its partial derivatives with respect to the joints. This means that the expression needs to include symbols referring to the joints, otherwise the derivatives will all be 0. This is usually achieved be using the forward kinematics expression, e.g.

root_T_tip = self.get_fk(self.root, self.tip)

You should also try to keep the expression as simple as possible. Casadi is able to compute the derivative of all functions defined in the wrapper through algorithmic differentiation, but some derivatives (e.g. for min or if) are not defined at all points. This means that your expression may not behave as expected. That is usually not a problem, but something to keep in mind. You might also accidentally create an expression whose derivative is 0 even though you included the forward kinematics.

upper and lower limit the rate of change of expression. A common pattern to implement a constraint is to let expression describe a position. In case of the pointing constraint this is a vector, e.g. the z axis of a camera in robot root frame, to include the forwards kinematics. Then you set upper=lower=goal-expression. This is essentially a position constraint.

weight should usually be one of two constants:

  • WEIGHT_ABOVE_CA
  • WEIGHT_BELOW_CA

If the weight is below the collision avoidance, it will keep a 5cm distance (or whatever is defined in the config file) from objects and with above it will be able to move the robot as close as 0cm to objects.

The weights don't describe a true priority, Giskard will try to minimize the sum of weight*error^2 of all constraints. This means that you need a sufficient distance between weights to create a priority. However, as the weights get bigger, the distance to the joint weights increases as well. If the absolute difference between joint weights and constraint weights becomes too high, the solver will become very sensitive to small errors and prone to shaking. Therefore, don't try to create to many weight levels and just stick to above and below collision avoidance. Important: Before solving the optimization problem, Giskard will remove all constraints which currently have a weight of 0. This means that during debugging, your constraint might not show up.

Step 3

Finally add your constraints to Giskard. Make sure to give it a unique name! You can use:

  • add_constraint
  • add_constraint_vector
  • add_vector_goal_constraints or similar

The first two are the low level interface to add constraints, but Goal defines a few wrappers for common patterns, like the third one.

Debugging

You can't look into constraints, while make_constraints is parsed, because you are only dealing with expressions. Instead use:

  • self.add_debug_expr
  • self.add_debug_matrix
  • self.add_debug_vector

Give it a unique name and any expression you want. You may want to use self.get_expr_velocity, to plot the derivative . Giskard will then create giskardpy/data/debug.pdf which plots the expression over time.