[Features Request] Shaft-shaft distance and Jacobian

[chewheel]

I'd like to submit an enhancement request to have the shaft-to-shaft collision avoidance added to the library. The constraint is inspired on Section V of

M. M. Marinho, B. V. Adorno, K. Harada and M. Mitsuishi, 
"Dynamic Active Constraints for Surgical Robots Using Vector-Field Inequalities," 
in IEEE Transactions on Robotics, vol. 35, no. 5, pp. 1166-1185, Oct. 2019, 
doi: 10.1109/TRO.2019.2920078.

from dqrobotics import *
from dqrobotics.utils import DQ_Geometry
from dqrobotics.robot_modeling import DQ_Kinematics
from numpy import linalg as la
from numpy import hstack as hs
from numpy import vstack as vs

# Collision avoidance between two line segments
# r1 means robot1, r2 means robot2
# toolbase1 means a dq point back off (length1) meters from tooltip1 along the axis of tool 1
# toolbase2 means a dq point back off (length2) meters from tooltip2 along the axis of tool 2
# something_dot mean the deriative of something with respect to time
# jabocian_q1_l1 means the jacobian matrix mapping robot1 joint velocities(q1_dot) onto tool1's line velocity(l1_dot)
# We just take out the q1 and the l1 and use them to specify the jacobian, so as other jacobian names are.
######################
# Aobut the jacobian's name rule,  jacobian_A_B @ A_dot = B_dot, so jacobian_A_B maps A_dot onto B_dot.
######################
class ShaftShaftCollisionAvoidance:
    def __init__(self, configuration):
        self.cfg = configuration
        self.closest_point1 = None
        self.closest_point2 = None
        self.W_ssca = None
        self.w_ssca = None
        self.this_loop_collision = None

    def ssca(self,
             l1_dq, jacobian_q1_l1,
             tooltip1_pose, jacobian_q1_tip1_pose,
             tooltip1, jacobian_q1_tip1,

             l2_dq, jacobian_q2_l2,
             tooltip2_pose, jacobian_q2_tip2_pose,
             tooltip2, jacobian_q2_tip2
             ):
        # Get the pose Jacobian and the pose at the intersection between the instrument's line and robot's raw end
        # in fact we should call that intersection point "effector's position"
        jacobian_q1_base1_pose = haminus8(DQ([1, 0, 0, 0, 0, 0, 0, -self.cfg.shaft1_length / 2.0])) @ jacobian_q1_tip1_pose
        jacobian_q2_base2_pose = haminus8(DQ([1, 0, 0, 0, 0, 0, 0, -self.cfg.shaft2_length / 2.0])) @ jacobian_q2_tip2_pose
        toolbase1_pose = tooltip1_pose * DQ([1, 0, 0, 0, 0, 0, 0, -self.cfg.shaft1_length / 2.0])
        toolbase2_pose = tooltip2_pose * DQ([1, 0, 0, 0, 0, 0, 0, -self.cfg.shaft2_length / 2.0])
        # get effector's translation jacobian and translation
        jacobian_q1_base1 = DQ_Kinematics.translation_jacobian(jacobian_q1_base1_pose, toolbase1_pose)
        jacobian_q2_base2 = DQ_Kinematics.translation_jacobian(jacobian_q2_base2_pose, toolbase2_pose)
        toolbase1 = translation(toolbase1_pose)
        toolbase2 = translation(toolbase2_pose)
        self.this_loop_collision = 0
        m1 = D(l1_dq)
        l1 = P(l1_dq)
        m2 = D(l2_dq)
        l2 = P(l2_dq)
        if l1 == l2 or l1 == -l2:
            # Here we do not specify the task onto which we map q1_dot, because the task changes in different situations
            jacobian_q1_task = DQ_Kinematics.line_to_line_distance_jacobian(jacobian_q1_l1, l1_dq, l2_dq)
            jacobian_q2_task = DQ_Kinematics.line_to_line_distance_jacobian(jacobian_q2_l2, l2_dq, l1_dq)
            D_task = DQ_Geometry.line_to_line_squared_distance(l1_dq, l2_dq)
        else:
            # Get two points(dq) on lines that are the closest to each other
            # Just formulas, I don't know the geometrical principle behind them ;^;
            l1xl2 = cross(l1, l2)
            self.closest_point1 = (cross(-m1, cross(l2, l1xl2)) + dot(m2, l1xl2) * l1) * (1.0 / la.norm(vec4(l1xl2))) ** 2
            self.closest_point2 = (cross(m2, cross(l1, l1xl2)) - dot(m1, l1xl2) * l2) * (1.0 / la.norm(vec4(l1xl2))) ** 2
            # Standard: depending on the relationship between tooltip and the closest point, define line's states
            distance_cp1_t1 = la.norm(vec3(tooltip1 - self.closest_point1))
            distance_cp1_b1 = la.norm(vec3(toolbase1 - self.closest_point1))
            distance_cp2_t2 = la.norm(vec3(tooltip2 - self.closest_point2))
            distance_cp2_b2 = la.norm(vec3(toolbase2 - self.closest_point2))
            if distance_cp1_t1 < self.cfg.shaft1_length and distance_cp1_b1 < self.cfg.shaft1_length:
                line1_state = "body"
            elif distance_cp1_t1 < distance_cp1_b1:
                line1_state = "tip"
            else:
                line1_state = "base"
            if distance_cp2_t2 < self.cfg.shaft2_length and distance_cp2_b2 < self.cfg.shaft2_length:
                line2_state = "body"
            elif distance_cp2_t2 < distance_cp2_b2:
                line2_state = "tip"
            else:
                line2_state = "base"

            if line1_state == "body" and line2_state == "body":
                jacobian_q1_task = DQ_Kinematics.line_to_line_distance_jacobian(jacobian_q1_l1, l1_dq, l2_dq)
                jacobian_q2_task = DQ_Kinematics.line_to_line_distance_jacobian(jacobian_q2_l2, l2_dq, l1_dq)
                D_task = DQ_Geometry.line_to_line_squared_distance(l1_dq, l2_dq)
            elif line1_state == "body" and line2_state == "tip":
                jacobian_q1_task = DQ_Kinematics.line_to_point_distance_jacobian(jacobian_q1_l1, l1_dq, tooltip2)
                jacobian_q2_task = DQ_Kinematics.point_to_line_distance_jacobian(jacobian_q2_tip2, tooltip2, l1_dq)
                D_task = DQ_Geometry.point_to_line_squared_distance(tooltip2, l1_dq)
            elif line1_state == "body" and line2_state == "base":
                jacobian_q1_task = DQ_Kinematics.line_to_point_distance_jacobian(jacobian_q1_l1, l1_dq, toolbase2)
                jacobian_q2_task = DQ_Kinematics.point_to_line_distance_jacobian(jacobian_q2_base2, toolbase2, l1_dq)
                D_task = DQ_Geometry.point_to_line_squared_distance(toolbase2, l2_dq)
            elif line1_state == "tip" and line2_state == "body":
                jacobian_q1_task = DQ_Kinematics.point_to_line_distance_jacobian(jacobian_q1_tip1, tooltip1, l2_dq)
                jacobian_q2_task = DQ_Kinematics.line_to_point_distance_jacobian(jacobian_q2_l2, l2_dq, tooltip1)
                D_task = DQ_Geometry.point_to_line_squared_distance(tooltip1, l2_dq)
            elif line1_state == "tip" and line2_state == "tip":
                jacobian_q1_task = DQ_Kinematics.point_to_point_distance_jacobian(jacobian_q1_tip1, tooltip1, tooltip2)
                jacobian_q2_task = DQ_Kinematics.point_to_point_distance_jacobian(jacobian_q2_tip2, tooltip2, tooltip1)
                D_task = DQ_Geometry.point_to_point_squared_distance(tooltip1, tooltip2)
            elif line1_state == "tip" and line2_state == "base":
                jacobian_q1_task = DQ_Kinematics.point_to_point_distance_jacobian(jacobian_q1_tip1, tooltip1, toolbase2)
                jacobian_q2_task = DQ_Kinematics.point_to_point_distance_jacobian(jacobian_q2_base2, toolbase2, tooltip1)
                D_task = DQ_Geometry.point_to_point_squared_distance(tooltip1, toolbase2)
            elif line1_state == "base" and line2_state == "body":
                jacobian_q1_task = DQ_Kinematics.point_to_line_distance_jacobian(jacobian_q1_base1, toolbase1, l2_dq)
                jacobian_q2_task = DQ_Kinematics.line_to_point_distance_jacobian(jacobian_q2_l2, l2_dq, toolbase1)
                D_task = DQ_Geometry.point_to_line_squared_distance(toolbase1, l2_dq)
            elif line1_state == "base" and line2_state == "tip":
                jacobian_q1_task = DQ_Kinematics.point_to_point_distance_jacobian(jacobian_q1_base1, toolbase1, tooltip2)
                jacobian_q2_task = DQ_Kinematics.point_to_point_distance_jacobian(jacobian_q2_tip2, tooltip2, toolbase1)
                D_task = DQ_Geometry.point_to_point_squared_distance(toolbase1, tooltip2)
            else:  # base, base
                jacobian_q1_task = DQ_Kinematics.point_to_point_distance_jacobian(jacobian_q1_base1, toolbase1, toolbase2)
                jacobian_q2_task = DQ_Kinematics.point_to_point_distance_jacobian(jacobian_q2_base2, toolbase2, toolbase1)
                D_task = DQ_Geometry.point_to_point_squared_distance(toolbase1, toolbase2)

        # In case of any accident, we always give the constraint of two tips collision avoidance.
        jacobian_q1_t2t = DQ_Kinematics.point_to_point_distance_jacobian(jacobian_q1_tip1, tooltip1, tooltip2)
        jacobian_q2_t2t = DQ_Kinematics.point_to_point_distance_jacobian(jacobian_q2_tip2, tooltip2, tooltip1)
        D_t2t = DQ_Geometry.point_to_point_squared_distance(tooltip1, tooltip2)
        D_t2t_error = D_t2t - self.cfg.safe_distance_for_lines ** 2

        # The inequality matrix and vector
        self.W_ssca = vs([hs([-jacobian_q1_task, -jacobian_q2_task]),
                          hs([-jacobian_q1_t2t, -jacobian_q2_t2t])])

        D_task_error = D_task - self.cfg.safe_distance_for_lines ** 2
        distance_criterion = D_task ** 0.5
        # Count the number of Collision avoidance
        if distance_criterion <= (self.cfg.shaft1_diameter + self.cfg.shaft2_diameter) / 2.0:
            self.this_loop_collision = 1

        self.w_ssca = self.cfg.eta_d_ssca * hs([D_task_error, D_t2t_error])

        return self.W_ssca, self.w_ssca, self.this_loop_collision

And

import numpy as np
from numpy import sin, cos, pi
from dqrobotics import *

def point_projected_onto_line(original_point, line):
    # Both point and line are dq number
    l = P(line)
    m = D(line)
    a_point_on_the_line = cross(l, m)
    result_point = DQ(vec3(a_point_on_the_line) + np.dot(vec3(original_point - a_point_on_the_line), vec3(l)) * vec3(l))
    return result_point

[mmmarinho]

@chewheel Thank you for the draft. I'll revise it and get back to you when I can.

[mmmarinho]

@chewheel Added in Python: b846cd6 CPP: 866f35e

Refer to https://github.com/dqrobotics/python-examples/tree/master/line_segment_to_line_segment_vfi

Importing the following

from dqrobotics import *
from dqrobotics.robot_modeling import DQ_Kinematics
from dqrobotics.utils import DQ_Geometry

will allow one to use

DQ_Kinematics.line_segment_to_line_segment_distance_jacobian(...)
DQ_Geometry.line_segment_to_line_segment_squared_distance(...)

Which are enough to properly generate the VFIs.

To support all of this development I also added

DQ_Geometry.closest_points_between_lines(...)
DQ_Geometry.is_line_segment(...)
DQ_Geometry.closest_points_between_line_segments(...)