SymbolManager

Symbolic Expressions

A key concept in Giskard are symbolic expressions. They are used, for example, to implement motion goals and expression monitors. To define expressions, a wrapper for the CasADi library is used. Here is an example:

import numpy as np

import giskardpy.casadi_wrapper as cas

a = cas.Symbol('a')
b = cas.Symbol('b')

expr = a + b
print(expr)
# (a+b)

expr2 = cas.greater(a, b)
print(expr2)
# (b<a)

expr_compiled = expr.compile(parameters=[a, b])
print(expr_compiled(a=1, b=2))
# [3.]

expr2_compiled = expr2.compile(parameters=[a, b])
print(expr2_compiled(a=1, b=2))
# [0.]

print(expr2_compiled.fast_call(np.array([2, 1])))
# [1.]

expr3 = a**2
expr3_gradient = cas.jacobian([expr3], [a])
print(expr3_gradient)
# (a+a)

Symbolic expressions are used for two reasons:

1. Giskard has to compute jacobians to form its Quadratic Program.
2. The evaluation of compiled expressions is practically constant and significantly faster than any python code.

Motion goals have to be defined with symbolic expressions because their jacobian has to be computed. Expression monitors use symbolic expressions to improve runtime.

The casadi wrapper also implements classes for common geometric entities:

import giskardpy.casadi_wrapper as cas

a_T_b = cas.TransMatrix()
b_T_c = cas.TransMatrix()

c_P_x = cas.Point3()
c_V_x = cas.Vector3()

a_T_c = a_T_b.dot(b_T_c)
a_P_x = a_T_c.dot(c_P_x)
a_V_x = a_T_c.dot(c_V_x)

Check the wrapper for a complete list of available functions.

Symbol Manager and God Map

Symbolic expressions are only useful, if their symbols refer to something meaningful. For that reason, we've implemented the symbol manager and the god map.
The symbol manager creates and keeps track of symbols for you and can provide input parameters for compiled functions.
The god map is a global singleton dict, where you can save any data. In conjunction, they work like this:

import giskardpy.casadi_wrapper as cas
from giskardpy.god_map import god_map
from giskardpy.symbol_manager import symbol_manager

god_map.a = 1
god_map.b = 2

a = symbol_manager.get_symbol('god_map.a')
b = symbol_manager.get_symbol('god_map.b')

expr = a + b
print(expr)
# (god_map.a+god_map.b)

expr2 = cas.greater(a, b)
print(expr2)
# (god_map.b<god_map.a)

expr_compiled = expr.compile()
args = symbol_manager.resolve_symbols(expr_compiled.str_params)
print(expr_compiled.fast_call(args))
# [3.]

expr2_compiled = expr2.compile()
args = symbol_manager.resolve_symbols(expr2_compiled.str_params)
print(expr2_compiled.fast_call(args))
# [0.]

god_map.b = -1
args = symbol_manager.resolve_symbols(expr2_compiled.str_params)
print(expr2_compiled.fast_call(args))
# [1.]

expr3 = a ** 2
expr3_gradient = cas.jacobian([expr3], [a])
print(expr3_gradient)
# (god_map.a+god_map.a)
expr3_gradient_compiled = expr3_gradient.compile()
args = symbol_manager.resolve_symbols(expr3_gradient_compiled.str_params)
print(expr3_gradient_compiled.fast_call(args))
# [2.]

The symbol manager can also handle some geometry messages or lists/numpy arrays using get_expr.

from geometry_msgs.msg import PoseStamped

from giskardpy.god_map import god_map
from giskardpy.symbol_manager import symbol_manager

god_map.muh = PoseStamped()
god_map.muh.pose.orientation.w = 1

pose = symbol_manager.get_expr('god_map.muh')
print(pose)
# @1=sq(god_map.muh.pose.orientation.w), 
# @2=sq(god_map.muh.pose.orientation.x), 
# @3=sq(god_map.muh.pose.orientation.y), 
# @4=sq(god_map.muh.pose.orientation.z), 
# @5=2, 
# @6=0, 
# [[(((@1+@2)-@3)-@4), (((@5*god_map.muh.pose.orientation.x)*god_map.muh.pose.orientation.y)-((@5*god_map.muh.pose.orientation.w)*god_map.muh.pose.orientation.z)), (((@5*god_map.muh.pose.orientation.x)*god_map.muh.pose.orientation.z)+((@5*god_map.muh.pose.orientation.w)*god_map.muh.pose.orientation.y)), god_map.muh.pose.position.x], 
#  [(((@5*god_map.muh.pose.orientation.x)*god_map.muh.pose.orientation.y)+((@5*god_map.muh.pose.orientation.w)*god_map.muh.pose.orientation.z)), (((@1-@2)+@3)-@4), (((@5*god_map.muh.pose.orientation.y)*god_map.muh.pose.orientation.z)-((@5*god_map.muh.pose.orientation.w)*god_map.muh.pose.orientation.x)), god_map.muh.pose.position.y], 
#  [(((@5*god_map.muh.pose.orientation.x)*god_map.muh.pose.orientation.z)-((@5*god_map.muh.pose.orientation.w)*god_map.muh.pose.orientation.y)), (((@5*god_map.muh.pose.orientation.y)*god_map.muh.pose.orientation.z)+((@5*god_map.muh.pose.orientation.w)*god_map.muh.pose.orientation.x)), (((@1-@2)-@3)+@4), god_map.muh.pose.position.z], 
#  [@6, @6, @6, 1]]


pose_compiled = pose.compile()
args = symbol_manager.resolve_symbols(pose_compiled.str_params)
print(pose_compiled.fast_call(args))
# [[1. 0. 0. 0.]
#  [0. 1. 0. 0.]
#  [0. 0. 1. 0.]
#  [0. 0. 0. 1.]]

god_map.muh.pose.position.x = 1
god_map.muh.pose.orientation.x = 1
god_map.muh.pose.orientation.w = 0

args = symbol_manager.resolve_symbols(pose_compiled.str_params)
print(pose_compiled.fast_call(args))
# [[ 1.  0.  0.  1.]
#  [ 0. -1.  0.  0.]
#  [ 0.  0. -1.  0.]
#  [ 0.  0.  0.  1.]]

WorldTree

Since you almost certainly want to create expressions that refer to something on the World Tree, it has several helpers functions, most notably:

root_link_T_tip_link = god_map.world.compose_fk_expression(self, root_link, tip_link)

To determine the symbols for a particular joint, you have to identify its name and do god_map.world.joints[joint_name].free_variables. For "normal" joints with one dof, this list will only have one entry.