class JointTrajPub(object):
def __init__(self):
"""
Publish trajectory_msgs::JointTrajectory for velocity control
"""
self._joint_traj_pub = rospy.Publisher('/vel_traj_controller/command',
JointTrajectory,
queue_size=10)
self._ctrl_conn = ControllersConnection(namespace="")
self._ctrl_conn.load_controllers("vel_traj_controller")
def set_init_pose(self, init_pose):
"""
Sets joints to initial position [0,0,0]
:return: The init Pose
"""
self.check_publishers_connection()
self.move_joints(init_pose)
def check_publishers_connection(self):
"""
Checks that all the publishers are working
:return:
"""
rate = rospy.Rate(1) # 1hz
while (self._joint_traj_pub.get_num_connections() == 0):
rospy.logdebug(
"No subscribers to vel_traj_controller yet so we wait and try again"
)
try:
self._ctrl_conn.start_controllers(
controllers_on="vel_traj_controller")
rate.sleep()
except rospy.ROSInterruptException:
# This is to avoid error when world is rested, time when backwards.
pass
rospy.logdebug("_joint_traj_pub Publisher Connected")
rospy.logdebug("All Joint Publishers READY")
def move_joints(self, joints_array):
vel_cmd = JointTrajectory()
vel_cmd.joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
vel_cmd.header.stamp = rospy.Time.now()
# create a JTP instance and configure it
jtp = JointTrajectoryPoint(positions=[0] * 6,
velocities=[0] * 6,
time_from_start=rospy.Duration(.0))
jtp.velocities = [
joints_array[0], joints_array[1], joints_array[2], joints_array[3],
joints_array[4], joints_array[5]
]
# setup the reset of the pt
vel_cmd.points = [jtp]
self._joint_traj_pub.publish(vel_cmd)
def jointTrajectoryCommand(self, joints_array):
rospy.loginfo("jointTrajectoryCommand")
try:
rospy.loginfo(rospy.get_rostime().to_sec())
while rospy.get_rostime().to_sec() == 0.0:
time.sleep(0.1)
rospy.loginfo(rospy.get_rostime().to_sec())
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.header.frame_id = "ur5"
jt.joint_names.append("shoulder_pan_joint")
jt.joint_names.append("shoulder_lift_joint")
jt.joint_names.append("elbow_joint")
jt.joint_names.append("wrist_1_joint")
jt.joint_names.append("wrist_2_joint")
jt.joint_names.append("wrist_3_joint")
dt = 0.01
p = JointTrajectoryPoint()
p.positions.append(joints_array[0])
p.positions.append(joints_array[1])
p.positions.append(joints_array[2])
p.positions.append(joints_array[3])
p.positions.append(joints_array[4])
p.positions.append(joints_array[5])
jt.points.append(p)
# set duration
jt.points[0].time_from_start = rospy.Duration.from_sec(dt)
rospy.loginfo("Test: velocities")
self._joint_traj_pub.publish(jt)
except rospy.ROSInterruptException:
pass
def start_loop(self, rate_value=2.0):
rospy.logdebug("Start Loop")
pos1 = [-2.873, 0.0, 6.28, 3.015, 1.21, 1.264, -0.97]
pos2 = [-2.873, 0.0, 6.28, 3.015, 1.21, 1.264, 0.97]
position = "pos1"
rate = rospy.Rate(rate_value)
while not rospy.is_shutdown():
if position == "pos1":
self.move_joints(pos1)
position = "pos2"
else:
self.move_joints(pos2)
position = "pos1"
rate.sleep()
def start_sinus_loop(self, rate_value=2.0):
rospy.logdebug("Start Loop")
w = 0.0
x = 1.0 * math.sin(w)
#pos_x = [0.0,0.0,x]
pos_x = [x, 0.0, 0.0]
#pos_x = [0.0, x, 0.0]
rate = rospy.Rate(rate_value)
while not rospy.is_shutdown():
self.move_joints(pos_x)
w += 0.05
x = 1.0 * math.sin(w)
#pos_x = [0.0, 0.0, x]
pos_x = [x, 0.0, 0.0]
#pos_x = [0.0, x, 0.0]
print(x)
rate.sleep()
class JointPub(object):
def __init__(self):
self.publishers_array = []
self._shoulder_pan_joint_pub = rospy.Publisher(
'/ur_shoulder_pan_vel_controller/command', Float64, queue_size=1)
self._shoulder_lift_joint_pub = rospy.Publisher(
'/ur_shoulder_lift_vel_controller/command', Float64, queue_size=1)
self._elbow_vel_joint_pub = rospy.Publisher(
'/ur_elbow_vel_controller/command', Float64, queue_size=1)
self._wrist_1_joint_pub = rospy.Publisher(
'/ur_wrist_1_vel_controller/command', Float64, queue_size=1)
self._wrist_2_joint_pub = rospy.Publisher(
'/ur_wrist_2_vel_controller/command', Float64, queue_size=1)
self._wrist_3_joint_pub = rospy.Publisher(
'/ur_wrist_3_vel_controller/command', Float64, queue_size=1)
self.publishers_array.append(self._shoulder_pan_joint_pub)
self.publishers_array.append(self._shoulder_lift_joint_pub)
self.publishers_array.append(self._elbow_vel_joint_pub)
self.publishers_array.append(self._wrist_1_joint_pub)
self.publishers_array.append(self._wrist_2_joint_pub)
self.publishers_array.append(self._wrist_3_joint_pub)
self._ctrl_conn = ControllersConnection(namespace="")
self._ctrl_conn.load_controllers("ur_shoulder_pan_vel_controller")
self._ctrl_conn.load_controllers("ur_shoulder_lift_vel_controller")
self._ctrl_conn.load_controllers("ur_elbow_vel_controller")
self._ctrl_conn.load_controllers("ur_wrist_1_vel_controller")
self._ctrl_conn.load_controllers("ur_wrist_2_vel_controller")
self._ctrl_conn.load_controllers("ur_wrist_3_vel_controller")
def set_init_pose(self, init_pose):
"""
Sets joints to initial position [0,0,0]
:return: The init Pose
"""
self.check_publishers_connection()
self.move_joints(init_pose)
def check_publishers_connection(self):
"""
Checks that all the publishers are working
:return:
"""
rate = rospy.Rate(1) # 1hz
while (self._shoulder_pan_joint_pub.get_num_connections() == 0):
rospy.logdebug(
"No susbribers to _shoulder_pan_joint_pub yet so we wait and try again"
)
try:
self._ctrl_conn.start_controllers(
controllers_on="ur_shoulder_pan_vel_controller")
rate.sleep()
except rospy.ROSInterruptException:
# This is to avoid error when world is rested, time when backwards.
pass
rospy.logdebug("_shoulder_pan_joint_pub Publisher Connected")
while (self._shoulder_lift_joint_pub.get_num_connections() == 0):
rospy.logdebug(
"No susbribers to _shoulder_lift_joint_pub yet so we wait and try again"
)
try:
self._ctrl_conn.start_controllers(
controllers_on="ur_shoulder_lift_vel_controller")
rate.sleep()
except rospy.ROSInterruptException:
# This is to avoid error when world is rested, time when backwards.
pass
rospy.logdebug("_shoulder_lift_joint_pub Publisher Connected")
while (self._elbow_vel_joint_pub.get_num_connections() == 0):
rospy.logdebug(
"No susbribers to _elbow_vel_joint_pub yet so we wait and try again"
)
try:
self._ctrl_conn.start_controllers(
controllers_on="ur_elbow_vel_controller")
rate.sleep()
except rospy.ROSInterruptException:
# This is to avoid error when world is rested, time when backwards.
pass
rospy.logdebug("_elbow_vel_joint_pub Publisher Connected")
while (self._wrist_1_joint_pub.get_num_connections() == 0):
rospy.logdebug(
"No susbribers to _wrist_1_joint_pub yet so we wait and try again"
)
try:
self._ctrl_conn.start_controllers(
controllers_on="ur_wrist_1_vel_controller")
rate.sleep()
except rospy.ROSInterruptException:
# This is to avoid error when world is rested, time when backwards.
pass
rospy.logdebug("_wrist_1_joint_pub Publisher Connected")
while (self._wrist_2_joint_pub.get_num_connections() == 0):
rospy.logdebug(
"No susbribers to _wrist_2_joint_pub yet so we wait and try again"
)
try:
self._ctrl_conn.start_controllers(
controllers_on="ur_wrist_2_vel_controller")
rate.sleep()
except rospy.ROSInterruptException:
# This is to avoid error when world is rested, time when backwards.
pass
rospy.logdebug("_wrist_2_joint_pub Publisher Connected")
while (self._wrist_3_joint_pub.get_num_connections() == 0):
rospy.logdebug(
"No susbribers to _wrist_3_joint_pub yet so we wait and try again"
)
try:
self._ctrl_conn.start_controllers(
controllers_on="ur_wrist_3_vel_controller")
rate.sleep()
except rospy.ROSInterruptException:
# This is to avoid error when world is rested, time when backwards.
pass
rospy.logdebug("_wrist_3_joint_pub Publisher Connected")
rospy.logdebug("All Joint Publishers READY")
def move_joints(self, joints_array):
i = 0
for publisher_object in self.publishers_array:
joint_value = Float64()
joint_value.data = joints_array[i]
rospy.logdebug("JointsPos>>" + str(joint_value))
publisher_object.publish(joint_value)
i += 1
def start_loop(self, rate_value=2.0):
rospy.logdebug("Start Loop")
pos1 = [-2.873, 0.0, 6.28, 3.015, 1.21, 1.264, -0.97]
pos2 = [-2.873, 0.0, 6.28, 3.015, 1.21, 1.264, 0.97]
position = "pos1"
rate = rospy.Rate(rate_value)
while not rospy.is_shutdown():
if position == "pos1":
self.move_joints(pos1)
position = "pos2"
else:
self.move_joints(pos2)
position = "pos1"
rate.sleep()
def start_sinus_loop(self, rate_value=2.0):
rospy.logdebug("Start Loop")
w = 0.0
x = 1.0 * math.sin(w)
#pos_x = [0.0,0.0,x]
pos_x = [x, 0.0, 0.0]
#pos_x = [0.0, x, 0.0]
rate = rospy.Rate(rate_value)
while not rospy.is_shutdown():
self.move_joints(pos_x)
w += 0.05
x = 1.0 * math.sin(w)
#pos_x = [0.0, 0.0, x]
pos_x = [x, 0.0, 0.0]
#pos_x = [0.0, x, 0.0]
print(x)
rate.sleep()