Robot Tasks

Move to Marker

To recognize the marker we need package called ar_track_alvar.

cd ~/catkin_ws/src
git clone https://github.com/ROSinTraining/ar_track_alvar.git
cd ..
catkin_make

Create launch file called ar.launch inside franka_gazebo/launch.

 1<?xml version="1.0"?>
 2<launch>
 3    <arg name="marker_size" default="11.5" />
 4    <arg name="max_new_marker_error" default="0.08" />
 5    <arg name="max_track_error" default="0.4" />
 6
 7    <arg name="cam_image_topic" default="/camera/rgb/image_raw" />
 8    <arg name="cam_info_topic" default="/camera/rgb/camera_info" />
 9    <arg name="output_frame" default="/camera_link" />
10
11    <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen">
12        <param name="marker_size"           type="double" value="$(arg marker_size)" />
13        <param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
14        <param name="max_track_error"       type="double" value="$(arg max_track_error)" />
15        <param name="output_frame"          type="string" value="$(arg output_frame)" />
16
17        <remap from="camera_image"  to="$(arg cam_image_topic)" />
18        <remap from="camera_info"   to="$(arg cam_info_topic)" />
19    </node>
20</launch>

Finally code for move to marker.

cd catkin_ws/src
catkin_create_pkg panda_controllers rospy
cd ..
catkin_make
mkdir -p src/panda_controllers/scripts
touch src/panda_controllers/scripts/move_to_marker.py
chmod +x src/panda_controllers/scripts/move_to_marker.py

Add the code to move_to_marker.py.

 1#!/usr/bin/env python3
 2
 3import sys
 4import rospy
 5import moveit_commander
 6import tf
 7import tf2_ros
 8from geometry_msgs.msg import Pose
 9from ar_track_alvar_msgs.msg import AlvarMarkers
10
11#declare global variables
12world_frame = "panda_link0"
13vel_scaling =  .3
14home_position = [.4, .0, .3]
15home_orientation = [.1, .0, .0, .0]
16marker_pose = None
17
18#convert marker from camera frame to robot's base frame
19def transform_pose(pose, target_frame):
20    if tf_listener.canTransform(target_frame, pose.header.frame_id, rospy.Time(0)):
21        #transform pose
22        transform = tf_listener.transformPose(target_frame, pose)
23
24        # try:
25        #     tran = tf2_ros.Buffer.lookup_transform("world", "ar_maker_1", rospy.Time(), rospy.Duration(10))
26
27        return transform.pose
28
29#callback function to receive marker messages
30def marker_cb(msg):
31    global marker_pose
32    if len(msg.markers) == 0:
33        return
34    marker = msg.markers[0]
35    marker.pose.header.frame_id = marker.header.frame_id
36    marker_pose = transform_pose(marker.pose, "panda_link0")
37
38#set Pose message through lists
39def set_pose(xyz = [0, 0, 0], q = [0, 0, 0, 1]):
40    pose = Pose()
41    pose.position.x = xyz[0]
42    pose.position.y = xyz[1]
43    pose.position.z = xyz[2]
44    pose.orientation.x = q[0]
45    pose.orientation.y = q[1]
46    pose.orientation.z = q[2]
47    pose.orientation.w = q[3]
48    return pose
49
50#confirm if plan should be executed
51def plan_accepted():
52    return input("Do you want to execute the plan [y] or replan [n]? ") == "y"
53
54#plan and execute to given pose; If plan is not confirmed plan again
55def plan_and_execute(group, pose):
56    group.set_pose_target(pose)
57    if plan_accepted():
58        group.go(wait=True)
59        group.stop()
60        group.clear_pose_targets()
61    else:
62        exit()
63
64#main function of application
65def main():
66    #initialize moveit
67    moveit_commander.roscpp_initialize(sys.argv)
68
69    # Your Code For Task 1 #
70    group = moveit_commander.MoveGroupCommander("panda_arm")
71    group.set_max_velocity_scaling_factor(vel_scaling)
72
73
74    #while loop to move the robot to the found AR marker
75    while not rospy.is_shutdown():
76        plan_and_execute(group, set_pose(home_position, home_orientation))
77        if marker_pose:
78            marker = [marker_pose.position.x, marker_pose.position.y, home_position[2]]
79            plan_and_execute(group, set_pose(marker))
80        else:
81            rospy.logwarn("No marker detected.")
82
83if __name__ == '__main__':
84    rospy.init_node('move_to_marker', anonymous=True)
85    tf_listener = tf.TransformListener()
86
87    # Your Code for Task 2 #
88    rospy.Subscriber("/ar_pose_marker", AlvarMarkers, marker_cb)
89
90    main()

Run the task. Please use new terminals for the every command.

Warning

Do not forget to source your catkin workspace.

roslaunch franka_gazebo panda.launch
roslaunch panda_moveit_config move_group.launch
roslaunch franka_gazebo ar.launch
rosrun panda_controllers move_to_marker.py

Go to Goal (Real Robot)

Hint

Please remember if your using different system for controlling robot, add export ROS_MASTER_URI=http://<ip>:11311 and export ROS_IP=<ip>.

Refere to Getting Started for setting up your robot.

roslaunch franka_control franka_control.launch robot_ip:=172.16.0.2 load_gripper:=true robot:=panda
roslaunch panda_moveit_config move_group.launch

Create go_to_goal.py inside panda_controllers package.

 1#!/usr/bin/env python3
 2
 3import sys
 4from tokenize import group
 5import rospy
 6import moveit_commander
 7import tf
 8import tf2_ros
 9from geometry_msgs.msg import Pose
10
11
12def main():
13    moveit_commander.roscpp_initialize(sys.argv)
14
15    group = moveit_commander.MoveGroupCommander("panda_arm")
16    group.set_max_velocity_scaling_factor(0.5)
17    group.set_max_acceleration_scaling_factor(0.3)
18    group.set_planning_pipeline_id('pilz_industrial_motion_planner')
19    group.set_planner_id('PTP')
20
21
22    target = Pose()
23    target.position.x = 0.4
24    target.position.y = 0.2
25    target.position.z = 0.5
26    target.orientation.x = 1
27    target.orientation.y = 0
28    target.orientation.z = 0
29    target.orientation.w = 0
30
31    target1 = Pose()
32    target1.position.x = 0.4
33    target1.position.y = - 0.2
34    target1.position.z = 0.5
35    target1.orientation.x = 1
36    target1.orientation.y = 0
37    target1.orientation.z = 0
38    target1.orientation.w = 0
39
40    while not rospy.is_shutdown():
41        group.set_pose_target(target, end_effector_link="panda_link8")
42        group.go()
43        group.stop()
44        group.clear_pose_targets()
45
46        group.set_pose_target(target1, end_effector_link="panda_link8")
47        group.go()
48        group.stop()
49        group.clear_pose_targets()
50
51
52if __name__ == '__main__':
53    rospy.init_node('go_to_goal', anonymous=True)
54
55    main()

Warning

Carefully test the robot position so that it do not collide with its environment. Also set

group.set_max_velocity_scaling_factor(0.5)
group.set_max_acceleration_scaling_factor(0.3)

low for the testing.