Cant evade static obstacules

[jcalderon12]

Hi, I am a Robocomp researcher at the University of Extremadura, Spain.

I am using the toolbox to calculate the velocities of the joints of a manipulator (Kinova Gen3) needed to approach a point in space avoiding a static cube but the solver cannot solve the Jacobian.

I install Pypi version 1.1.1

To reproduce just it need to execute this script with python3:

import time

import swift
import roboticstoolbox as rtb
import spatialmath as sm
import numpy as np
import qpsolvers as qp
# spatialgeometry is a utility package for dealing with geometric objects
import spatialgeometry as sg
# typing utilities
from typing import Tuple

## Launch the simulator Swift
env = swift.Swift()
env.launch(realtime=True)
#env = rtb.backends.PyPlot.PyPlot()
#env.launch(realtime=True)

# Create a KionovaGen3 robot object
kinova = rtb.models.KinovaGen3()
print(kinova.grippers)
# Set joint angles to ready configuration
kinova.q = kinova.qr


# Add the robot to the simulator
env.add(kinova)
#kinova = rtb.models.Panda()

# axes
goal_axes = sg.Axes(0.1)
ee_axes = sg.Axes(0.1)

# Add the axes to the environment
env.add(ee_axes)
env.add(goal_axes)
ee_axes.T = kinova.fkine(kinova.q)

# Number of joint in the Kinova which we are controlling
n = 7

# objects
cup = sg.Cylinder(0.05, 0.1, pose=sm.SE3.Trans(0.4, 0.4, 0), color=(0, 0, 1))
cube = sg.Cuboid((0.1, 0.1, 0.7), pose=sm.SE3.Trans(0.2, 0.2, 0.3), color=(1, 0, 0))
env.add(cup)
env.add(cube)

collisions = [cube]

# Set the desired end-effector pose
rot = kinova.fkine(kinova.q).R
rot = sm.SO3.OA([1, 0, 0], [0, 0, -1])
Tep = sm.SE3.Rt(rot, [0.4, 0.4, 0.1])   # green = x-axis, red = y-axis, blue = z-axis
goal_axes.T = Tep

arrived = False
dt = 0.05

env.step(0)
time.sleep(5)

while not arrived:
    # The current pose of the kinova's end-effector
    Te = kinova.fkine(kinova.q)

    # Transform from the end-effector to desired pose
    eTep = Te.inv() * Tep

    # Spatial error
    e = np.sum(np.abs(np.r_[eTep.t, eTep.rpy() * np.pi / 180]))

    # Calulate the required end-effector spatial velocity for the robot
    # to approach the goal. Gain is set to 1.0
    v, arrived = rtb.p_servo(Te, Tep, 1.0, threshold=0.01)

    # Gain term (lambda) for control minimisation
    Y = 0.01

    # Quadratic component of objective function
    Q = np.eye(n + 6)

    # Joint velocity component of Q
    Q[:n, :n] *= Y

    # Slack component of Q
    Q[n:, n:] = (1 / e) * np.eye(6)

    # The equality contraints
    Aeq = np.c_[kinova.jacobe(kinova.q), np.eye(6)]
    beq = v.reshape((6,))

    # The inequality constraints for joint limit avoidance
    Ain = np.zeros((n + 6, n + 6))
    bin = np.zeros(n + 6)

    # The minimum angle (in radians) in which the joint is allowed to approach
    # to its limit
    ps = 0.05

    # The influence angle (in radians) in which the velocity damper
    # becomes active
    pi = 0.9

    # Form the joint limit velocity damper
    Ain[:n, :n], bin[:n] = kinova.joint_velocity_damper(ps, pi, n)

    for collision in collisions:

        # Form the velocity damper inequality contraint for each collision
        # object on the robot to the collision in the scene
        c_Ain, c_bin = kinova.link_collision_damper(
            collision,
            kinova.q[:n],
            0.3,
            0.05,
            1.0,
            start=kinova.link_dict["half_arm_1_link"],
            end=kinova.link_dict["end_effector_link"],
        )

        # If there are any parts of the robot within the influence distance
        # to the collision in the scene
        if c_Ain is not None and c_bin is not None:
            c_Ain = np.c_[c_Ain, np.zeros((c_Ain.shape[0], 6))]

            # Stack the inequality constraints
            Ain = np.r_[Ain, c_Ain]
            bin = np.r_[bin, c_bin]

    # Linear component of objective function: the manipulability Jacobian
    c = np.r_[-kinova.jacobm(kinova.q).reshape((n,)), np.zeros(6)]

    # The lower and upper bounds on the joint velocity and slack variable
    qdlim = [2.175, 2.175, 2.175, 2.175, 2.61, 2.61, 2.61]      # inventadas
    lb = -np.r_[qdlim, 10 * np.ones(6)]
    ub = np.r_[qdlim, 10 * np.ones(6)]

    # Solve for the joint velocities dq
    qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub, solver='piqp')

    print(qd)

    # Apply the joint velocities to the kinova
    kinova.qd[:n] = qd[:n]

    # Update the ee axes
    ee_axes.T = Te

    # Step the simulator by 50 ms
    env.step(dt)
# Uncomment to stop the browser tab from closing
env.hold()

Output:
Traceback (most recent call last):
File "/home/robolab/Descargas/Telegram Desktop/corke_obstacule.py", line 144, in
kinova.qd[:n] = qd[:n]
TypeError: 'NoneType' object is not subscriptable

This is what i obtain when i use the script and this video show it

Grabacion.de.pantalla.desde.31-10-24.11.30.42.webm

I'm using ubuntu 22.04 and python 3