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
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
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
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 aFollowJointTrajectory
action server and you need to write a client for it.Asked by gvdhoorn on 2019-05-08 13:09:55 UTC