Tutorials
Building a Robot Model
Before TriPs functionality can be used on a robot, it first has to be build within TriP. This Tutorial will show how to build the TriPed robot shown below:
Here each leg was highlighted in a different color. Since all legs are identicall, this tutorial will start with a single leg. More information about the triped legs can be found here .
The first step in setting up a robot is identifying the groups and transformations. TriP uses Groups to model closed kinematic chains. These are structures where multiple moving parts converge in a single location forming one or more loops. Some examples can be seen down below:
These closed chains will either be connected directly to each other or using a series of other moving parts. Such a series is called a open kinematic chain. Open Kinematic chains are handled using a series of transformations.
In the case of the TriPed the following chains can be identified:
This in turn means that the leg of the TriPed contains one kinematic group and a number of transformations representing the open chain.
Once each group has been identified the group construction worklow goes like this:
Building open chains
Groups handle closed chains by abstracting them into a virtual open chains that models how group moves combined with two mapping functions from these virtual joints to the actual actuated joints and back. This means that for both the open and the closed chain, a chain has to be build. The precise transformations depend on the type of robot and its conventions. The kinematic transformations of the TriPed are described here: here . This leads to the following virtual open chain:
1from math import radians
2from typing import Dict
3from casadi import SX, nlpsol, vertcat
4import numpy as np
5
6
7from trip_kinematics.KinematicGroup import KinematicGroup
8from trip_kinematics.Transformation import Transformation
9from trip_kinematics.Robot import Robot
10from trip_kinematics.Utility import hom_translation_matrix, x_axis_rotation_matrix
11from trip_kinematics.Utility import y_axis_rotation_matrix, z_axis_rotation_matrix
12from trip_kinematics.Utility import hom_rotation, get_translation
13a_ccs_p_trans = Transformation(name='A_ccs_P_trans',
14 values={'tx': 0.265, 'tz': 0.014})
15a_ccs_p_rot = Transformation(name='gimbal_joint',
16 values={'rx': 0, 'ry': 0, 'rz': 0},
17 state_variables=['rx', 'ry', 'rz'],
18 parent=a_ccs_p_trans)
And the corresponding transformations for the open chain:
1a_p_ll = Transformation(name='A_P_LL',
2 values={'tx': 1.640, 'tz': -0.037, },
3 parent=closed_chain)
4a_ll_zero = Transformation(name='zero_angle_convention',
5 values={'ry': radians(-3)},
6 parent=a_p_ll)
7a_ll_zero_ll_joint = Transformation(name='extend_joint',
8 values={'ry': 0},
9 state_variables=['ry'],
10 parent=a_ll_zero)
11a_ll_joint_fcs = Transformation(name='A_LL_Joint_FCS',
12 values={'tx': -1.5},
13 parent=a_ll_zero_ll_joint)
Warning
Note that the second code block references the closed chain as its parent. Since this group is not yet build the code above will encounter errors. In Practice the close chain group first has to be built. The correct order of this code can be seen [here](https://github.com/TriPed-Robot/trip_kinematics/blob/main/src/trip_robots/triped_leg.py).
Note that both chains are made up of transformations without state_variables. Such transformations are ‘static’ and dont represent a joint. It is possible to construct open chains without such transformations (see the denavit hartenberg convention for example). In practice however they are handy to specify the position of a joint at a specified angle. This allows the joint angles to be interpretable. This can be seen with the a_ll_zero transformation. It ensures that a extend_joint angle corresponds to the foot of the leg being completely retracted.
Create Mappings
For the kinematic group a mapping from the actuated swing_joints to the virtual gimbal joint have to be provided. These joints can be seen down below:
The mapping between these joints can be computet by solving a geometric closing equation. As pictured above, the tip \(x_i\) the output lever connected to the actuated joints \(i\) alwas intersects the sphere at position \(c_i\) where \(i\) is either 1 or 2.
Mathematically this can be described using:
Where \(rx,ry,rz\) are the rotation around the x, y and z axis and the suberscript :math:`v,~a`denotes the virtual and actuated state respectively.
The virtual_to_actuated and actuated_to_virtual mappings can now be defined as the virtual or actuated state that solves the closure equation assuming the other state as a fixed value. This can be done using casadis nonelinear programming solver. The final code can be seen down below:
1def sphere_centers(r_x, r_y, r_z):
2 """Generates the centers of the spheres used for the geometric closure equation
3
4 Args:
5 r_x (float): gimbal joint state rx
6 r_y (float): gimbal joint state ry
7 r_z (float): gimbal joint state rz
8
9 Returns:
10 numpy array, numpy array: Two 3D arrays containing the sphere centers
11 """
12 a_ccs_p_trans_m = hom_translation_matrix(
13 t_x=0.265, t_y=0, t_z=0.014)
14 a_ccs_p_rot_m = hom_rotation(x_axis_rotation_matrix(r_x) @
15 y_axis_rotation_matrix(r_y) @
16 z_axis_rotation_matrix(r_z))
17 a_p_sph_1_2 = hom_translation_matrix(
18 t_x=0.015, t_y=0.029, t_z=-0.0965)
19 a_p_sph_2_2 = hom_translation_matrix(
20 t_x=0.015, t_y=-0.029, t_z=-0.0965)
21
22 a_ccs_ = a_ccs_p_trans_m @ a_ccs_p_rot_m
23 a_c1 = a_ccs_ @ a_p_sph_1_2
24 a_c2 = a_ccs_ @ a_p_sph_2_2
25
26 return get_translation(a_c1), get_translation(a_c2)
27
28
29def intersection_left(theta):
30 """calculates the desired sphere intersection point based on the left swing joint
31
32 Args:
33 theta (float): angle of the left swing joint
34
35 Returns:
36 numpy array: the sphere intersection
37 """
38 a_ccs_lsm_trans = hom_translation_matrix(
39 t_x=0.139807669447128, t_y=0.0549998406976098, t_z=-0.051)
40 a_ccs_lsm_rot = hom_rotation(z_axis_rotation_matrix(radians(-345.0)))
41 a_mcs_1_joint = hom_rotation(z_axis_rotation_matrix(theta))
42 a_mcs_1_sp_1_1 = hom_translation_matrix(
43 t_x=0.085, t_y=0, t_z=-0.0245)
44
45 a_ccs_sp_1_1 = a_ccs_lsm_trans @ a_ccs_lsm_rot @ a_mcs_1_joint @ a_mcs_1_sp_1_1
46 return get_translation(a_ccs_sp_1_1)
47
48
49def intersection_right(theta):
50 """calculates the desired sphere intersection point based on the right swing joint
51
52 Args:
53 theta (float): angle of the right swing joint
54
55 Returns:
56 numpy array: the sphere intersection
57 """
58 a_ccs_rsm_tran = hom_translation_matrix(
59 t_x=0.139807669447128, t_y=-0.0549998406976098, t_z=-0.051)
60 a_ccs_rsm_rot = hom_rotation(z_axis_rotation_matrix(radians(-15.0)))
61 a_mcs_2_joint = hom_rotation(z_axis_rotation_matrix(theta))
62 a_mcs_2_sp_2_1 = hom_translation_matrix(
63 t_x=0.085, t_y=0, t_z=-0.0245)
64
65 a_ccs_sp_2_1 = a_ccs_rsm_tran @ a_ccs_rsm_rot @ a_mcs_2_joint @ a_mcs_2_sp_2_1
66 return get_translation(a_ccs_sp_2_1)
67
68
69def swing_to_gimbal(state: Dict[str, float], tips: Dict[str, float] = None):
70 """Actuated to virtual state mapping for the TriPed legss closed subchain
71
72 Args:
73 state (Dict[str, float]): actuated state of the TriPed leg closed subchain
74 tips (Dict[str, float], optional): Initial state for the closure equation solver.
75 Defaults to None in which case [0,0,0] is used.
76
77 Returns:
78 Dict[str, Dict[str, float]]: the correspdonding state of the virtual chain
79 """
80 x_0 = [0, 0, 0]
81 if tips:
82 x_0[2] = tips['rx']
83 x_0[3] = tips['ry']
84 x_0[4] = tips['rz']
85
86 nlp = {'x': virtual_state, 'f': closing_equation, 'p': actuated_state}
87 nlp_solver = nlpsol('swing_to_gimbal', 'ipopt', nlp, opts)
88 solution = nlp_solver(x0=x_0,
89 p=[state['swing_left'], state['swing_right']])
90 sol_vector = np.array(solution['x'])
91 return {'gimbal_joint': {'rx': sol_vector[0][0],
92 'ry': sol_vector[1][0],
93 'rz': sol_vector[2][0]}}
94
95
96def gimbal_to_swing(state: Dict[str, Dict[str, float]], tips: Dict[str, float] = None):
97 """Virtual to actuated state mapping for the TriPed legss closed subchain
98
99 Args:
100 state (Dict[str, Dict[str, float]]): virtual state of the TriPed leg closed subchain
101 tips (Dict[str, float], optional): Initial state for the closure equation solver.
102 Defaults to None in which case [0,0] is used.
103
104 Returns:
105 Dict[str, float]: the correspdonding actuated state
106 """
107 x_0 = [0, 0]
108 continuity = 0
109 if tips:
110 x_0[0] = tips['swing_left']
111 x_0[1] = tips['swing_right']
112 continuity = (x_0-actuated_state).T @ (x_0-actuated_state)
113
114 nlp = {'x': actuated_state, 'f': closing_equation+continuity, 'p': virtual_state,
115 'g': actuated_state[0]*actuated_state[1]}
116 nlp_solver = nlpsol('gimbal_to_swing', 'ipopt', nlp, opts)
117 solution = nlp_solver(x0=x_0,
118 p=[state['gimbal_joint']['rx'],
119 state['gimbal_joint']['ry'],
120 state['gimbal_joint']['rz']],
121 ubg=0)
122 sol_vector = np.array(solution['x'])
123 return {'swing_left': sol_vector[0][0], 'swing_right': sol_vector[1][0]}
124
125
126theta_left = SX.sym('theta_left')
127theta_right = SX.sym('theta_right')
128gimbal_x = SX.sym('gimbal_x')
129gimbal_y = SX.sym('gimbal_y')
130gimbal_z = SX.sym('gimbal_z')
131
132virtual_state = vertcat(gimbal_x, gimbal_y, gimbal_z)
133actuated_state = vertcat(theta_left, theta_right)
134
135opts = {'ipopt.print_level': 0, 'print_time': 0}
136RADIUS = 0.11
137c1, c2 = sphere_centers(r_x=gimbal_x, r_y=gimbal_y, r_z=gimbal_z)
138closing_equation = (((c1-intersection_left(theta_right)).T @ (c1-intersection_left(theta_right)) -
139 RADIUS**2)**2 +
140 ((c2-intersection_right(theta_left)).T @ (c2-intersection_right(theta_left)) -
141 RADIUS**2)**2)
Building the group
Using both the mappings and the virtual open chain, the group can be build:
1closed_chain = KinematicGroup(name='closed_chain',
2 virtual_chain=[a_ccs_p_trans, a_ccs_p_rot],
3 actuated_state={
4 'swing_left': 0, 'swing_right': 0},
5 actuated_to_virtual=swing_to_gimbal,
6 virtual_to_actuated=gimbal_to_swing)
Note that the closed chain specifies no parent since it is located directly at the robots base and the open chain specifies no mappings since these are autogenerated.
Combining groups to a robot
The last step is to combine the group and transformations into a robot object:
triped_leg = Robot([closed_chain,a_p_ll,a_ll_zero,a_ll_zero_ll_joint,a_ll_joint_fcs])
Building the complete robot
Since the TriPed has three legs, the above process has to be repeated three times. Each time the joints and actuated state need their own unique names.
Since this would be tedious to do by hand, a function can be written that returns the full transformation of a single leg. This functions then follows the above steps, only appending the initial kinematic group with a transformation to the start position of each leg.
This function can be seen here:
1def leg_model(leg_number: str):
2 """Helper function that constructs each TriPed leg as a list of Transformations.
3
4 Args:
5 leg_number (str): The leg number which determins the orientation.
6 Acceptable values [0,1,2]
7 """
8 def rename_swing_to_gimbal(swing: Dict[str, float], tips: Dict[str, float] = None):
9 swing = deepcopy(swing)
10 swing['swing_left'] = swing[leg_name+'swing_left']
11 swing['swing_right'] = swing[leg_name+'swing_right']
12 del swing[leg_name+'swing_left']
13 del swing[leg_name+'swing_right']
14
15 if tips is not None:
16 tips = deepcopy(tips)
17 tips['gimbal_joint'] = tips[leg_name+'gimbal_joint']
18 del tips[leg_name+'gimbal_joint']
19
20 gimbal = swing_to_gimbal(swing, tips)
21
22 gimbal[leg_name+'gimbal_joint'] = gimbal['gimbal_joint']
23 del gimbal['gimbal_joint']
24
25 return gimbal
26
27 def rename_gimbal_to_swing(gimbal: Dict[str, float], tips: Dict[str, float] = None):
28 gimbal = deepcopy(gimbal)
29 gimbal['gimbal_joint'] = gimbal[leg_name+'gimbal_joint']
30 del gimbal[leg_name+'gimbal_joint']
31
32 if tips is not None:
33 tips = deepcopy(tips)
34 tips['swing_left'] = tips[leg_name+'swing_left']
35 tips['swing_right'] = tips[leg_name+'swing_right']
36 del tips[leg_name+'swing_left']
37 del tips[leg_name+'swing_right']
38
39 swing = gimbal_to_swing(gimbal, tips)
40
41 swing[leg_name+'swing_left'] = swing['swing_left']
42 swing[leg_name+'swing_right'] = swing['swing_right']
43 del swing['swing_left']
44 del swing['swing_right']
45
46 return swing
47
48 leg_name = 'leg_'+str(leg_number)+'_'
49
50 leg_rotation = Transformation(name=leg_name+'leg_rotation',
51 values={'rz': -1*radians(120)*leg_number})
52 a_ccs_p_trans = Transformation(name=leg_name+'A_ccs_P_trans',
53 values={'tx': 0.265, 'tz': 0.014},
54 parent=leg_rotation)
55 a_ccs_p_rot = Transformation(name=leg_name+'gimbal_joint',
56 values={'rx': 0, 'ry': 0, 'rz': 0},
57 state_variables=['rx', 'ry', 'rz'],
58 parent=a_ccs_p_trans)
59
60 closed_chain = KinematicGroup(name=leg_name+'closed_chain',
61 virtual_chain=[leg_rotation,
62 a_ccs_p_trans, a_ccs_p_rot],
63 actuated_state={
64 leg_name+'swing_left': 0, leg_name+'swing_right': 0},
65 actuated_to_virtual=rename_swing_to_gimbal,
66 virtual_to_actuated=rename_gimbal_to_swing)
67
68 a_p_ll = Transformation(name=leg_name+'A_P_LL',
69 values={'tx': 1.640, 'tz': -0.037, },
70 parent=closed_chain)
71 a_ll_zero = Transformation(name=leg_name+'zero_angle_convention',
72 values={'ry': radians(-3)},
73 parent=a_p_ll)
74 a_ll_zero_ll_joint = Transformation(name=leg_name+'extend_joint',
75 values={'ry': 0},
76 state_variables=['ry'],
77 parent=a_ll_zero)
78 a_ll_joint_fcs = Transformation(name=leg_name+'A_LL_Joint_FCS',
79 values={'tx': -1.5},
80 parent=a_ll_zero_ll_joint)
81
82 return [closed_chain, a_ll_zero_ll_joint, a_ll_joint_fcs, a_p_ll, a_ll_zero]
The final TriPed robot can then be build using:
1triped = Robot(leg_model(0)+leg_model(1)+leg_model(2))
Importing URDF Files
The Universal Robot Description Format (URDF) is used to describe a variety of serial robot mechanisms. Trip includes a URDF parser that allows the importation of alist of transfomrations from a URDF file
It can be used directly after importing the TriP library by calling the function trip_kinematics.from_urdf with the path to the URDF file as the argument. Note that the function returns a list of Transformations, which you probably want to create a Robot from in most cases:
transformations_list = trip_kinematics.urdf_parser.from_urdf(urdf_path)
robot = Robot(transformations_list)
This means you can also add other Transformations manually on top of those specified in the URDF file, if required.
Also note that the parser includes defaults for certain values if the corresponding URDF tag is missing, specifically:
<origin> defaults to <origin xyz=”0 0 0” rpy=”0 0 0” />.
(The same also applies if only one of xyz and rpy is specified, with the omitted value defaulting to “0 0 0”)
<axis> defaults to <axis xyz=”0 0 1” />