Skip to content
Snippets Groups Projects
Commit da3401b0 authored by Jan Weber's avatar Jan Weber
Browse files

Added test node to test the driver

parent 9f0ba9ee
No related branches found
No related tags found
No related merge requests found
...@@ -123,5 +123,14 @@ Download the [your robot]_support and [your robot]_movit_config packages for the ...@@ -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> <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):
![motion download](./doc/rviz_motion_download.png) ![motion streaming](./doc/rviz_motion_streaming.png)
<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>
#!/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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment