publishing joint trajectory msgs on gazebo

asked 2019-05-07 13:47:34 -0600

joeced gravatar image

updated 2019-05-07 18:31:09 -0600

jayess gravatar image

hello i have a delta robot that is controlled using joint trajectory. i created 1 publisher for the inverse kinematics and a subscriber to receiive the result . i want those result to be published on gazebo directly. i am facing those problems:

[ERROR] [1557254010.564552520, 268.353000000]: Exception thrown when deserializing message of length [104] from [/inverse_sub]: Buffer Overrun
[ERROR] [1557254010.565049509, 268.353000000]: Exception thrown when deserializing message of length [16] from [/inverse_sub]: Buffer Overrun
[ERROR] [1557254010.565280432, 268.353000000]: Exception thrown when deserializing message of length [8] from [/inverse_sub]: Buffer Overrun

the following is my subscriber.py file :

#!/usr/bin/env python

import rospy
from std_msgs.msg import Float64MultiArray
from std_msgs.msg import Time
from std_msgs.msg import Header
from std_msgs.msg import Duration
import numpy as np
from math import *
import time
from matplotlib.pyplot import *
from trajectory_msgs.msg import JointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint

sqrt3 = sqrt(3)
sin120 = sqrt(3) / 2
cos120 = -0.5
tan60 = sqrt(3)
sin30 = 0.5
tan30 = 1 / sqrt(3)

default_drob_dimensions = {'R':0.40941, 'r':0.4, 'l':0.8, 'd':0.07845}
c_default_drob_dimensions = {'R':0.40941, 'r':0.4, 'l':0.8, 'd':0.07845}
a = 0
b = 0
c = 0
secs = 0
nsecs = 1


def s_inv_kinematics(x0, y0, z0, drob_dimensions = default_drob_dimensions):
    """
    Takes the position of the end effector as input and returns
    the corresponding angle in degree if existing
    else it returns "position does not exist"
    """

    # Robot dimensions
    d = drob_dimensions['d']
    R = drob_dimensions['R']
    r = drob_dimensions['r']
    l = drob_dimensions['l']

    e = 2 * sqrt(3) * d
    f = 2 * sqrt(3) * R
    re = l
    rf = r

    y1 = -0.5 * 0.57735 * f
    y0 -= 0.5 * 0.57735 * e

    a = (x0 * x0 + y0 * y0 + z0 * z0 + rf * rf - re * re - y1 * y1) / (2 * z0)
    b = (y1 - y0) / z0

    d = -(a + b * y1) * (a + b * y1) + rf * (b * b * rf + rf)
    if (d < 0):
        return "position does not exist"

    yj = (y1 - a * b - sqrt(d)) / (b * b + 1)
    zj = a + b * yj

    if (yj > y1):
        toadd = 180
    else:
        toadd = 0
    theta = 180 * atan(-zj / (y1 - yj)) / pi + toadd
    return theta





def inv_kinematics(x0, y0, z0, drob_dimensions = default_drob_dimensions):
    """
    Takes the position of the end effector as input and returns
    the 3 angles in degree if existing
    else it returns "position does not exist"
    """

    # Robot dimensions
    d = drob_dimensions['d']
    R = drob_dimensions['R']
    r = drob_dimensions['r']
    l = drob_dimensions['l']

    e = 2 * sqrt(3) * d
    f = 2 * sqrt(3) * R
    re = l
    rf = r

    theta1 = s_inv_kinematics(x0, y0, z0, drob_dimensions)
    theta2 = s_inv_kinematics(x0 * cos120 + y0 * sin120, y0 * cos120 - x0 * sin120, z0, drob_dimensions)
    theta3 = s_inv_kinematics(x0 * cos120 - y0 * sin120, y0 * cos120 + x0 * sin120, z0, drob_dimensions)
    if (
            theta1 == "position does not exist" or theta2 == "position does not exist" or theta3 == "position does not exist"):
        print ("position does not exist")

    print (theta1, theta2, theta3)


    theta1 *= 3.14/180
    theta2 *= 3.14/180  
    theta3 *= 3.14/180

    a = theta1
    b = theta2
    c = theta3

    pub1 = rospy.Publisher ("/delta_robot/delta_robot_controller/command", JointTrajectory, queue_size=10)  
    pub2 = rospy.Publisher ("/delta_robot/delta_robot_controller/command", JointTrajectoryPoint, queue_size=10 ...
(more)
edit retag flag offensive close merge delete

Comments

Why are you creating 5 different publishers for the single action server /delta_robot/delta_robot_controller/command?

gvdhoorn gravatar imagegvdhoorn ( 2019-05-08 03:11:20 -0600 )edit

because i am sending a joint trajectory that requires all those data and each data has a type of message. am i doing wrong? do you have a solution or an example that can help me?

joeced gravatar imagejoeced ( 2019-05-08 12:04:16 -0600 )edit

because i am sending a joint trajectory that requires all those data and each data has a type of message.

that's not how things work. You don't have to create publishers for each field separately. Just create a single one per message.

But in this case, you'll want to actionlib and the Python Simple Action client, as that /delta_robot/delta_robot_controller actually exposes a FollowJointTrajectory action server and you need to write a client for it.

gvdhoorn gravatar imagegvdhoorn ( 2019-05-08 13:09:55 -0600 )edit