Pose transformation

The first argument, p_from, is used to transform the second argument, p_from_to, and the result is then returned. This means that the result is the resulting pose, when starting at the coordinate system of p_from, and then in that coordinate system moving p_from_to.

This function can be seen in two different views. Either the function transforms, that is translates and rotates, p_from_to by the parameters of p_from. Or the function is used to get the resulting pose, when first making a move of p_from and then from there, a move of p_from_to.

If the poses were regarded as transformation matrices, it would look like:

T_world->to = T_world->from * T_from->to T_x->to = T_x->from * T_from->to

Parameters

p_from: starting pose (spatial vector)

p_from_to: pose change relative to starting pose (spatial vector)

Return Value

resulting pose (spatial vector)

Example command: pose_trans(p[.2,.5,.1,1.57,0,0], p[.2,.5,.6,1.57,0,0])

• Example Parameters:
  • p_1 = p[.2,.5,.1,1.57,0,0] → The first point
  • p_2 = p[.2,.5,.6,1.57,0,0] → The second point
    • Returns p[0.4,-0.0996,0.60048,3.14,0.0,0.0]