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)