diff --git a/README.md b/README.md
index f8fb65ac349952f82c338f620b812c86b559b376..e22e541f66efbe82122c5c8be990f83b8eb63562 100644
--- a/README.md
+++ b/README.md
@@ -123,5 +123,14 @@ Download the [your robot]_support and [your robot]_movit_config packages for the
<include file="$(find [your robot]_moveit_config)/launch/moveit_planning_execution_egm.launch"></include>
+## Test the driver
+The Python script **abb_egm_driver/scripts/driver_test_node.py** tests the driver by alternately sending trajectories via motion download and real-time streamed motions to the real robot. Before using it, check carefully that the motions performed in the script can be executed collision-free by your robot. To run the script:
+
+ roslaunch abb_egm_driver driver_test.launch
+
+Now the robot should move alternately from one side of a virtual tower to the other (motion download) and perform a small circle in the XZ plane on each side (motion streaming):
+
+ 
+
diff --git a/abb_egm_driver/launch/driver_test.launch b/abb_egm_driver/launch/driver_test.launch
new file mode 100644
index 0000000000000000000000000000000000000000..7f39e07b7290072d347806f7d95b16c9b8d655af
--- /dev/null
+++ b/abb_egm_driver/launch/driver_test.launch
@@ -0,0 +1,7 @@
+<launch>
+
+ <include file="$(find abb_irb1600_6_12_moveit_config)/launch/moveit_planning_execution_egm.launch">
+ </include>
+
+ <node pkg="abb_egm_driver" type="driver_test_node.py" name="driver_test_node" output="screen"></node>
+</launch>
diff --git a/abb_egm_driver/scripts/driver_test_node.py b/abb_egm_driver/scripts/driver_test_node.py
new file mode 100755
index 0000000000000000000000000000000000000000..501f7f1e65eef4d63255044bf7bc1eba12ae5c02
--- /dev/null
+++ b/abb_egm_driver/scripts/driver_test_node.py
@@ -0,0 +1,200 @@
+#!/usr/bin/env python3
+import sys
+import math
+from math import pi
+import rospy
+import moveit_commander
+import moveit_msgs
+import geometry_msgs
+import trajectory_msgs
+
+class StateValidity():
+ def __init__(self):
+ # prepare service for collision check
+ self.sv_srv = rospy.ServiceProxy('/check_state_validity', moveit_msgs.srv.GetStateValidity)
+ # wait for service to become available
+ self.sv_srv.wait_for_service()
+ rospy.loginfo('StateValidity service is available')
+
+
+ def robotStateValid(self, robot_state, group_name='manipulator', constraints=None):
+ '''
+ Given a RobotState and a group name and an optional Constraints
+ return the validity of the State
+ '''
+ # prepare request
+ gsvr = moveit_msgs.srv.GetStateValidityRequest()
+ gsvr.robot_state = robot_state
+ gsvr.group_name = group_name
+ if constraints != None:
+ gsvr.constraints = constraints
+
+ # call service
+ result = self.sv_srv.call(gsvr)
+ return result
+
+class ForwardKinematics():
+ def __init__(self):
+ # prepare service for forward kinematics calculation
+ self.fk_srv = rospy.ServiceProxy('/compute_fk', moveit_msgs.srv.GetPositionFK)
+ # wait for service to become available
+ self.fk_srv.wait_for_service()
+ rospy.loginfo('GetPositionFK service is available')
+
+
+ def getPose(self, robot, robot_state, link="tool0",reference_frame_id="base_link"):
+ '''
+ Calculate the pose of the link related to the
+ frame reference_frame_id for the given
+ robot state robot_state.
+ Return type is geometry_msgs.msg.Pose
+ '''
+ # prepare request
+ gfkr = moveit_msgs.srv.GetPositionFKRequest()
+ gfkr.header.frame_id = reference_frame_id
+ gfkr.fk_link_names = robot.get_link_names()
+ gfkr.robot_state = robot_state
+
+ # call service
+ result = self.fk_srv.call(gfkr)
+
+ for link_name,pose_stamped in zip(result.fk_link_names,result.pose_stamped):
+ if link_name == link:
+ return pose_stamped.pose
+
+class InverseKinematics():
+ def __init__(self):
+ # prepare service for inverse kinematics calculation
+ self.ik_srv = rospy.ServiceProxy('/compute_ik', moveit_msgs.srv.GetPositionIK)
+ # wait for service to become available
+ self.ik_srv.wait_for_service()
+ rospy.loginfo('GetPositionIK service is available')
+
+
+ def getJointAngles(self, goal, current_robot_state, group_name='manipulator',link="tool0"):
+ '''
+ Calculate the joint angles for the given goal.
+ current_robot_state is the seed state for the IK solver.
+ Return type is moveit_msgs.msg.RobotState
+ '''
+ # prepare request
+ gikr = moveit_msgs.srv.GetPositionIKRequest()
+ gikr.ik_request.group_name = group_name
+ current_robot_state.is_diff = True
+ gikr.ik_request.robot_state = current_robot_state
+ gikr.ik_request.ik_link_name = link
+ gikr.ik_request.avoid_collisions = False
+ gikr.ik_request.pose_stamped = goal
+ gikr.ik_request.timeout = rospy.Duration(1.0)
+
+ # call service
+ result = self.ik_srv.call(gikr)
+ if(result.error_code.val != 1):
+ print("Error: ", result.error_code.val)
+
+ return result.solution
+
+
+def move_to_other_side(move_group):
+ '''
+ Test code that moves the robot to the other side of the tower.
+ The trjaectory is gnerated and executed by MoveIt.
+ Motion downloading is used.
+ '''
+ joint_goal = move_group.get_current_joint_values()
+ if(joint_goal[0] > 0):
+ joint_goal[0] = -pi/8
+ else:
+ joint_goal[0] = pi/8
+ joint_goal[1] = 0
+ joint_goal[2] = 0
+ joint_goal[3] = 0
+ joint_goal[4] = pi/4
+ joint_goal[5] = 0
+
+ move_group.set_joint_value_target(joint_goal)
+ move_group.set_max_velocity_scaling_factor(1.0)
+ move_group.go(joint_goal,wait=True)
+ move_group.stop()
+
+def circle(move_group, robot):
+ '''
+ Test code that makes a circular movement in the XZ plane
+ with a radius of 10cm starting from the current robot position.
+ Motion streaming is used.
+ '''
+ inverseKinematics = InverseKinematics()
+ forwardKinematics = ForwardKinematics()
+ joint_command_publisher = rospy.Publisher('/joint_command', trajectory_msgs.msg.JointTrajectoryPoint, queue_size=10)
+
+ # Set the current position as a static target for one second so that the driver has definitely
+ # switched to motion streaming mode when positions are streamed to the robot afterwards.
+ jtp = trajectory_msgs.msg.JointTrajectoryPoint()
+ jtp.positions = move_group.get_current_joint_values()
+ for _ in range(10):
+ joint_command_publisher.publish(jtp)
+ rospy.sleep(0.1)
+
+ current_robot_state = robot.get_current_state()
+ current_pose = forwardKinematics.getPose(robot,current_robot_state)
+ goal = geometry_msgs.msg.PoseStamped()
+ goal.header.frame_id = "base_link"
+ goal.header.stamp = rospy.Time.now()
+ goal.pose.orientation = current_pose.orientation
+ goal.pose.position.y = current_pose.position.y
+ r = 0.1
+ mx = current_pose.position.x - r # so that the start point of the circle is on the current robot position
+ mz = current_pose.position.z
+ steps = 40
+ t = 3
+ for a in range(steps):
+ dx = r * math.cos(a/steps*2.0*pi)
+ dz = r * math.sin(a/steps*2.0*pi)
+ goal.pose.position.x = mx + dx
+ goal.pose.position.z = mz + dz
+ robot_state = inverseKinematics.getJointAngles(goal, current_robot_state)
+ jtp = trajectory_msgs.msg.JointTrajectoryPoint()
+ jtp.positions = robot_state.joint_state.position
+ joint_command_publisher.publish(jtp)
+ rospy.sleep(t/steps)
+
+def test():
+ # prepare moveit_commander interface, robot, scene and move_group
+ moveit_commander.roscpp_initialize(sys.argv)
+ rospy.init_node('rob_app', anonymous=True)
+ robot = moveit_commander.RobotCommander()
+ scene = moveit_commander.PlanningSceneInterface()
+ group_name = "manipulator"
+ move_group = moveit_commander.MoveGroupCommander(group_name)
+
+ # an virtual obstacle in the robot scene
+ box_pose = geometry_msgs.msg.PoseStamped()
+ box_pose.header.frame_id = "base_link"
+ box_pose.pose.orientation.w = 1.0
+ box_pose.pose.position.z = 1.0
+ box_pose.pose.position.x = 0.6
+ box_name = "box"
+ scene.add_box(box_name, box_pose, size=(0.1, 0.1, 2.0))
+
+ # wait until the real robot sends data
+ while True:
+ joints = move_group.get_current_joint_values()
+ if len(joints) > 0:
+ break
+ print("Waiting for incoming data from robot...")
+ rospy.sleep(1.0)
+
+ # execute tests
+ while not rospy.is_shutdown():
+ move_to_other_side(move_group)
+ rospy.sleep(0.5)
+ circle(move_group, robot)
+ rospy.sleep(0.5)
+
+ move_group.stop()
+
+if __name__ == '__main__':
+ try:
+ test()
+ except rospy.ROSInterruptException:
+ pass