Robotics StackExchange | Archived questions

publishing joint trajectory msgs on gazebo

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) 
    pub3 = rospy.Publisher ("/delta_robot/delta_robot_controller/command", Header, queue_size=10) 
    pub4 = rospy.Publisher ("/delta_robot/delta_robot_controller/command", Time, queue_size=10) 
    pub5 = rospy.Publisher ("/delta_robot/delta_robot_controller/command", Duration, queue_size=10) 

    msg = JointTrajectory()
    msg1 = JointTrajectoryPoint()
    msg2 = Header()
    msg3 = Time()
    msg4 = Duration()

    msg2.seq = 0
    msg2.frame_id = ''
    msg3.data = rospy.Time(0, 0)
    msg.joint_names = ["motor_1_uleg_1", "motor_2_uleg_2", "motor_3_uleg_3"]
    msg1.positions = [a, b, c]
    msg1.velocities = [0, 0, 0]
    msg1.accelerations = [0, 0, 0]
    msg1.effort = [0] 
    msg4.data = rospy.Duration(0, 1)
    msg.points.append(msg1)
    pub1.publish(msg)
    pub2.publish(msg1)
    pub3.publish(msg2)
    pub4.publish(msg3)
    pub5.publish(msg4)
    return [theta1, theta2, theta3]

def receive_data(msg):
    print ("The values received are: ")
    print (msg.data)
    x0 = msg.data[0]
    y0 = msg.data[1]
    z0 = msg.data[2]
    inv_kinematics(x0, y0, z0, default_drob_dimensions)

if __name__=='__main__':

    rospy.init_node('inverse_sub')
    sub = rospy.Subscriber("inverse_Kinematics", Float64MultiArray, receive_data)
    while not rospy.is_shutdown()
        rospy.spin()

can anyone help?

Asked by joeced on 2019-05-07 13:47:34 UTC

Comments

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

Asked by gvdhoorn on 2019-05-08 03:11:20 UTC

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?

Asked by joeced on 2019-05-08 12:04:16 UTC

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.

Asked by gvdhoorn on 2019-05-08 13:09:55 UTC

Answers

You don't need to publish multiple times

here is an example of using different message types:

pub = rospy.Publisher('/delta_robot/delta_robot_controller/command', JointTrajectory, queue_size=10)

joints_str = JointTrajectory()
joints_str.header = Header()
joints_str.header.stamp = rospy.Time.now()
joints_str.joint_names = ['motor_1_uleg_1', 'motor_2_uleg_2', 'motor_3_uleg_3']
point=JointTrajectoryPoint()
point.positions = [var1, var2, var3]
point.time_from_start = rospy.Duration(2)
joints_str.points.append(point)

pub.publish(joints_str)
rospy.loginfo("position updated")

Asked by Moneera Banjar on 2020-10-14 08:38:08 UTC

Comments