parser.add_argument('--node-name',
dest='nodeName',
default='ur_driver',
help='Specifies the name of the node.')
arguments, unknown = parser.parse_known_args()
rospy.init_node(arguments.nodeName, disable_signals=True)
jointPrefix = rospy.get_param('prefix', '')
if jointPrefix:
print('Setting prefix to %s' % jointPrefix)
robot = Robot()
nodeName = arguments.nodeName + '/' if arguments.nodeName != 'ur_driver' else ''
jointStatePublisher = JointStatePublisher(robot, jointPrefix, nodeName)
trajectoryFollower = TrajectoryFollower(robot, jointStatePublisher,
jointPrefix, nodeName)
trajectoryFollower.start()
# we want to use simulation time for ROS
clockPublisher = rospy.Publisher('clock', Clock, queue_size=1)
if not rospy.get_param('use_sim_time', False):
rospy.logwarn('use_sim_time is not set!')
timestep = int(robot.getBasicTimeStep())
while robot.step(timestep) != -1 and not rospy.is_shutdown():
jointStatePublisher.publish()
trajectoryFollower.update()
# pulish simulation clock
msg = Clock()
time = robot.getTime()
from rosgraph_msgs.msg import Clock
rospy.init_node('ur_driver', disable_signals=True)
jointPrefix = rospy.get_param('prefix', '')
if jointPrefix:
print('Setting prefix to %s' % jointPrefix)
else:
print('Not setting')
gripperjointPrefix = rospy.get_param('prefix', '')
robot = Robot()
jointStatePublisher = JointStatePublisher(robot, jointPrefix)
trajectoryFollower = TrajectoryFollower(robot, jointStatePublisher,
jointPrefix, gripperjointPrefix)
trajectoryFollower.start()
# we want to use simulation time for ROS
clockPublisher = rospy.Publisher('clock', Clock, queue_size=1)
if not rospy.get_param('use_sim_time', False):
rospy.logwarn('use_sim_time is not set!')
timestep = int(robot.getBasicTimeStep())
while robot.step(timestep) != -1 and not rospy.is_shutdown():
jointStatePublisher.publish()
trajectoryFollower.update()
grippertrajectoryFollower.update()
# pulish simulation clock
msg = Clock()
rospy.init_node(arguments.nodeName, disable_signals=True)
jointPrefix = rospy.get_param('prefix', '')
if jointPrefix:
print('Setting prefix to %s' % jointPrefix)
robot = Robot()
nodeName = arguments.nodeName + '/' if arguments.nodeName != 'ur_driver' else ''
jointStatePublisher = JointStatePublisher(robot, jointPrefix, nodeName)
# These joints must match with the list in ros_controllers.yaml!
armJointNames = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
armTrajectoryFollower = TrajectoryFollower(robot, jointStatePublisher,
jointPrefix, armJointNames,
'arm_controller')
armTrajectoryFollower.start()
# These joints must match with the list in ros_controllers.yaml!
handJointNames = [
'finger_1_joint_1', 'finger_2_joint_1', 'finger_middle_joint_1',
'palm_finger_1_joint', 'palm_finger_2_joint'
]
handTrajectoryFollower = TrajectoryFollower(robot, jointStatePublisher,
jointPrefix, handJointNames,
'hand_controller')
handTrajectoryFollower.start()
# we want to use simulation time for ROS
clockPublisher = rospy.Publisher('clock', Clock, queue_size=1)
if not rospy.get_param('use_sim_time', False):
arguments, unknown = parser.parse_known_args()
rospy.init_node(arguments.nodeName, disable_signals=True)
jointPrefix = rospy.get_param('prefix', '')
if jointPrefix:
print('Setting prefix to %s' % jointPrefix)
robot = Robot()
nodeName = arguments.nodeName + '/' if arguments.nodeName != 'ur_driver' else ''
jointStatePublisher = JointStatePublisher(robot, jointPrefix, nodeName)
gripperCommander1 = GripperCommander(robot, jointStatePublisher, jointPrefix,
'panda_1')
gripperCommander2 = GripperCommander(robot, jointStatePublisher, jointPrefix,
'panda_2')
trajectoryFollower1 = TrajectoryFollower(robot, jointStatePublisher,
jointPrefix, 'panda_1')
trajectoryFollower2 = TrajectoryFollower(robot, jointStatePublisher,
jointPrefix, 'panda_2')
trajectoryFollower1.start()
trajectoryFollower2.start()
gripperCommander1.start()
gripperCommander2.start()
init_pos = {
"panda_1_joint1": 0.000,
"panda_1_joint2": -0.785,
"panda_1_joint3": 0.0,
"panda_1_joint4": -2.356,
"panda_1_joint5": 0.0,
"panda_1_joint6": 1.57,
"panda_1_joint7": 0.4,
"panda_2_joint1": 0.000,
def setUp(self):
self.traj_follower = TrajectoryFollower()
class TrajectoryFollowingTestCase(unittest.TestCase):
def setUp(self):
self.traj_follower = TrajectoryFollower()
def tearDown(self):
self.traj_follower = None
def test_1_straight_line(self):
# Initial conditions
x_0 = -0.5
y_0 = 0.5
theta_0 = 0
X_0 = [x_0, y_0, theta_0] # Initial state
# Generate trajectory ------------------------------
# Trajectory to follow - list of waypoints
traj = []
list_x = np.linspace(0, 20, 200)
i = 0
for x in list_x:
wp = Waypoint()
wp.x = x
wp.y = x
wp.i = i
traj.append(wp)
i += 1
# -------------------------------------------------
traj_x, traj_y, traj_theta = self.traj_follower.follow_traj(
X_0, traj, params)
TrackedPathPlotter.path_visualizer(traj_x, traj_y, X_0, traj)
def test_3(self):
# Initial conditions
x_0 = 0
y_0 = 0
theta_0 = 0
X_0 = [x_0, y_0, theta_0] # Initial state
# Generate trajectory ------------------------------
# Trajectory to follow - list of waypoints
traj = []
list_x = np.linspace(0, 20, 200)
i = 0
# print list_x
# wp = Waypoint()
for x in list_x[0:50]:
wp = Waypoint()
wp.x = x
wp.y = 0
wp.i = i
traj.append(wp)
i += 1
for x in list_x[50:]:
wp = Waypoint()
wp.x = x
wp.y = 2
wp.i = i
traj.append(wp)
i += 1
# -------------------------------------------------
traj_x, traj_y, traj_theta = self.traj_follower.follow_traj(
X_0, traj, params)
TrackedPathPlotter.path_visualizer(traj_x, traj_y, X_0, traj)
def test_3_sin(self):
# Initial conditions
x_0 = 0
y_0 = 1
theta_0 = 0
X_0 = [x_0, y_0, theta_0] # Initial state
# Generate trajectory ------------------------------
# Trajectory to follow - list of waypoints
traj = []
list_x = np.linspace(0, 20, 200)
i = 0
# print list_x
# wp = Waypoint()
for x in list_x:
wp = Waypoint()
wp.x = x
wp.y = np.sin(x)
wp.i = i
traj.append(wp)
i += 1
# -------------------------------------------------
traj_x, traj_y, traj_theta = self.traj_follower.follow_traj(
X_0, traj, params)
TrackedPathPlotter.path_visualizer(traj_x, traj_y, X_0, traj)