Joint_trajectory_controller do not work

[davidedema]

Hi, i'm using humble, when i send joint trajectories to the /joint_trajectory_controller/joint_trajectory topic it doesn't work proprerly. No matter the joint config i send it will always go max velocity in the floor, penetrating it and crashing gazebo. This is the code: motion.py

import rclpy
from rclpy.node import Node

from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
import trajectory as traj
from std_msgs.msg import Header
from rclpy.duration import Duration
import numpy as np
import kinematics as kin

class PointPublisher(Node):
    def __init__(self):
        super().__init__('point_publisher')
        self.publisher_ = self.create_publisher(JointTrajectory, '/joint_trajectory_controller/joint_trajectory', 10)
        self.q0 = [-1.57, -1.57, -1.57, -1.57, 0, 0]

    def publish_point(self, qi, qdi):
        msg = JointTrajectory()
        msg.header = Header()
        msg.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']

        j = 0
        dj = 0.1
        for i in range(len(qi[0,:])):
            point = JointTrajectoryPoint()
            msg.header.stamp = self.get_clock().now().to_msg()
            joints = qi[:,i].flatten()
            vel = qdi[:,i].flatten()
            point.positions = joints.tolist()
            point.velocities = vel.tolist()
            point.time_from_start = Duration(seconds=j).to_msg()
            j += dj
            msg.points.append(point)
        self.publisher_.publish(msg)

    def move(self, pos, orientation):
        rotm = kin.eul2Rot(orientation)
        T = np.identity(4)
        T[:3, :3] = rotm
        T[:3, 3] = pos
        possibleQ = kin.inverse_kinematics(T)
        # find closest q to q0
        qf = self.findClosestQ(possibleQ)
        qi, qdi, qddi = traj.cubic_trajectory_planning(self.q0, qf, np.zeros(6), np.zeros(6))
        self.publish_point(qi, qdi)
        # for i in range(len(qi[0,:])):
        #     self.publish_point(qi[:,i].flatten())
        self.q0 = qf

    def eucledeanDistance(self, q1, q2):
        return np.linalg.norm(q1 - q2)

    def findClosestQ(self, q):
        minDistance = float('inf')
        closestQ = None
        for q_ in q:
            distance = self.eucledeanDistance(self.q0, q_)
            if distance < minDistance:
                minDistance = distance
                closestQ = q_

        return closestQ

def main(args=None):
    rclpy.init(args=args)
    pointPub = PointPublisher()
    pointPub.move(np.array([-0.1, -0.1, 0.8]), np.array([0, -1.57, 0]))
    rclpy.spin(pointPub)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

trajectory.py

import numpy as np
import kinematics as kin

def cubic_trajectory_planning(q0 , qf, qd0, qdf, m=100):

    n = len(q0)

    a0 = np.copy(q0)
    a1 = np.zeros(n)
    a2 = 3 * (qf - q0) - 2 * qd0 - qdf
    a3 = -2 * (qf - q0) + qd0 + qdf

    timesteps = np.linspace(0, 1, m)

    q = np.zeros((n, m))
    qd = np.zeros((n, m))
    qdd = np.zeros((n, m))

    for i in range(len(timesteps)):
        t = timesteps[i]
        t_2 = t**2
        t_3 = t**3
        q[:, i] = a0 + a1 * t + a2 * t_2 + a3 * t_3
        qd[:, i] = a1 + 2 * a2 * t + 3 * a3 * t_2
        qdd[:, i] = 2 * a2 + 6 * a3 * t

    return q, qd, qdd

def main():
    pos = np.zeros(3)
    rot = np.zeros(3)

    input_pos = input("Enter position: ")
    input_rot = input("Enter rotation: ")

    rotm = kin.eul2Rot(rot)
    T = np.identity(4)
    T[:3, :3] = rotm
    T[:3, 3] = pos

    q0 = np.array([0, 0, 0, 0, 0, 0])
    qf = np.array([1, 1, 1, 1, 1, 1])
    qd0 = np.array([0, 0, 0, 0, 0, 0])
    qdf = np.array([0, 0, 0, 0, 0, 0])
    q, qd, qdd = cubic_trajectory_planning(q0, qf, qd0, qdf)
    print(q[0,:])

if __name__ == '__main__':
    main()

kinematics.py has kin function such as forward kin and inverse one. I'm pretty sure that trajectory and kin are correct. Now i'm not sure that i'm using the trajectory controller in the right way (it's first time).

Gazebo view:

Rviz view: