Robot Playground project

The robot playground project contains Python and C++ examples to help you get started with the Robot Learning Lab. Follow the steps below to install, build, run and finally submit your first project!

Installation

If you haven’t already setup your RLL workspace with the robot playground project, do so now by following the instructions outlined in the Getting started guide.

Building and running the project

Refer to the Development workflow document for detailed information on how to build and run the project. As a quick recap, here are the required commands to build and run the project:

catkin build rll_robot_playground_project

To start the simulation environment run:

roslaunch rll_robot_playground_project setup_simulation.launch

Run your python code, by executing:

roslaunch rll_robot_playground_project run_your_code.launch

And to finally submit your code, run:

roslaunch rll_robot_playground_project submit_project.launch

The project files are located in the ~/rll_ws/src/rll_robot_playground_project directory.

Hello World Example

To demonstrate how to use all the movement methods described in RLL MoveClient we will build a full example that utilizes all of them. The code is extended step by step as more methods are added. The examples are snippets of the whole program and each example should run on its own. For each step, the relevant new lines are highlighted.

Note

The example created below is the hello world program from the Robot Playground Project.

It is recommended that you follow along and copy the changes as they are added into the file scripts/playground.py or src/playground.cpp, respectively. This way you can recreate the example for yourself, run the code after every change and get a better understanding of what is happening.

Hello ROS

Python
C++
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
#! /usr/bin/env python
# -*- coding: utf-8 -*-

from __future__ import print_function

from math import pi
import time

import rospy
from geometry_msgs.msg import Pose, Point

from rll_move_client.client import RLLDefaultMoveClient
from rll_move_client.util import orientation_from_rpy


def hello_world(move_client):
    print("Hello world")  # avoid using print() for logging
    rospy.loginfo("Hello ROS")  # better use rospy.loginfo(), logerror()...

    return True


def main():
    rospy.init_node('hello_world')
    client = RLLDefaultMoveClient(hello_world)
    client.spin()


if __name__ == "__main__":
    main()

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
#include <geometry_msgs/Pose.h>
#include <ros/ros.h>

#include <rll_move_client/move_client_default.h>
#include <rll_move_client/util.h>

bool helloWorld(RLLDefaultMoveClient* const move_client)
{
  std::cout << "Hello World" << std::endl;  // avoid cout for logging
  ROS_INFO("Hello ROS");                    // better use ROS_INFO, ROS_ERROR...

  return true;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "hello_world");
  RLLCallbackMoveClient<RLLDefaultMoveClient> client(&helloWorld);
  client.spin();

  return 0;
}

We use the Getting started template and add some simple logging output. As the comments indicate, you should use the logging methods provided by ROS, instead of the system default output.

It’s moving!

The previous example didn’t actually move the robot. Lets change that!

Python
C++
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
#! /usr/bin/env python
# -*- coding: utf-8 -*-

from __future__ import print_function

from math import pi
import time

import rospy
from geometry_msgs.msg import Pose, Point

from rll_move_client.client import RLLDefaultMoveClient
from rll_move_client.util import orientation_from_rpy


def hello_world(move_client):
    # move to a random pose, this service call requires no arguments
    move_client.move_random()

    # The robot should now be moving (in RViz)! The delays in this code
    # are only for illustrative purposes and can be removed
    time.sleep(2)

    return True


def main():
    rospy.init_node('hello_world')
    client = RLLDefaultMoveClient(hello_world)
    client.spin()


if __name__ == "__main__":
    main()

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
#include <geometry_msgs/Pose.h>
#include <ros/ros.h>

#include <rll_move_client/move_client_default.h>
#include <rll_move_client/util.h>

bool helloWorld(RLLDefaultMoveClient* const move_client)
{
  // move to a random pose, this service call requires no arguments
  move_client->moveRandom();

  // The robot should now be moving (in RViz)! The delays in this code
  // are only for illustrative purposes and can be removed
  ros::Duration(2).sleep();

  return true;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "hello_world");
  RLLCallbackMoveClient<RLLDefaultMoveClient> client(&helloWorld);
  client.spin();

  return 0;
}

We use the move_random function to move the robot into a random position. Adding a delay between movements helps to see the different movements better.

Specifying joint angles

The next example illustrates how to add error checks, which we previously neglected.

Python
C++
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
#! /usr/bin/env python
# -*- coding: utf-8 -*-

from __future__ import print_function

from math import pi
import time

import rospy
from geometry_msgs.msg import Pose, Point

from rll_move_client.client import RLLDefaultMoveClient
from rll_move_client.util import orientation_from_rpy


def hello_world(move_client):
    # move all seven joints into their zero position by calling the move_joints
    # service and passing the joint values as arguments
    rospy.loginfo("calling move_joints with all joint values = 0")
    move_client.move_joints(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)

    time.sleep(2)

    # rotate the fourth joint by 90 degrees (pi/2 since we work with radians)
    rospy.loginfo("calling move_joints with joint_4 = pi/2")
    resp = move_client.move_joints(0.0, 0.0, 0.0, pi / 2, 0.0, 0.0, 0.0)

    # previously we neglected to check the response of the service call.
    # You should always check the result of a service call. If a critical
    # failure occurs during the service call an exception will be raised.
    # If you do not check the result of service call you might miss a non
    # critical failure, because i.e. you passed an invalid pose
    if not resp:
        rospy.logwarn("move_joints service call failed (as expected)")

    # ups, moving the fourth joint by 90 degrees didn't work, we bumped into
    # the workspace boundaries
    # Let's try to move joint 2, 4 and 6 so that we end up in an upright
    # position of the end effector close to the front of the workspace.
    rospy.loginfo("calling move_joints to move into an upright position "
                  "close to the front of the workspace")
    resp = move_client.move_joints(0.0, pi / 4, 0.0, -pi / 4,
                                   0.0, -pi / 2, 0.0)

    if not resp:
        rospy.logerr("move_joints service call failed (unexpectedly)!")

    time.sleep(2)

    return True


def main():
    rospy.init_node('hello_world')
    client = RLLDefaultMoveClient(hello_world)
    client.spin()


if __name__ == "__main__":
    main()

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
#include <geometry_msgs/Pose.h>
#include <ros/ros.h>

#include <rll_move_client/move_client_default.h>
#include <rll_move_client/util.h>

bool helloWorld(RLLDefaultMoveClient* const move_client)
{
  // move all seven joints into their zero (initial) position
  // set all joint values of the Request to zero (unnecessary, zero is the default)
  ROS_INFO("calling move_joints with all joint values = 0");
  move_client->moveJoints(0, 0, 0, 0, 0, 0, 0);

  ros::Duration(2).sleep();

  // rotate the fourth joint by 90 degrees (pi/2 since we work with radians)
  // the remaining joint values are still equal to zero
  ROS_INFO("calling move_joints with joint_4 = pi/2");
  bool success;
  success = move_client->moveJoints(0, 0, 0, M_PI / 2, 0, 0, 0);

  // previously we neglected to check the response of the service call.
  // You should always check the result of a service call!
  if (!success)
  {
    ROS_ERROR("move_joints service call failed (as expected)");
  }

  // ups, moving the fourth joint by 90 degrees didn't work, we bumped into
  // the workspace boundaries
  // Let's try to move joint 2, 4 and 6 so that we end up in an upright position
  // of the end effector close to the front of the workspace.
  ROS_INFO("calling move_joints to move into an upright position close to the front of the workspace");
  success = move_client->moveJoints(0, M_PI / 4, 0, -M_PI / 4, 0, -M_PI / 2, 0);

  if (!success)
  {
    ROS_ERROR("move_joints service call failed (unexpectedly)");
  }

  ros::Duration(2).sleep();

  return true;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "hello_world");
  RLLCallbackMoveClient<RLLDefaultMoveClient> client(&helloWorld);
  client.spin();

  return 0;
}

We use the move_joints function to specify the seven joint positions. As stated in Error handling, service calls return False on failure. In this example the second call move_client.move_joints(0.0, 0.0, 0.0, pi / 2, 0.0, 0.0, 0.0) would move the robot outside the allowed workspace. As a result the call fails and no movement is made. Without the error check you might not have noticed that the call failed!

Hint

If you check the output log you will notice that the failure has been reported there, too. Service call failures will always be logged, regardless of your own error checks.

More movement

Python
C++
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
#! /usr/bin/env python
# -*- coding: utf-8 -*-

from __future__ import print_function

from math import pi
import time

import rospy
from geometry_msgs.msg import Pose, Point

from rll_move_client.client import RLLDefaultMoveClient
from rll_move_client.util import orientation_from_rpy


def hello_world(move_client):
    # moving by specifying joint angle values is not the most intuitive way
    # it's easier to specify the pose of the end effector we'd like to reach
    goal_pose = Pose()
    goal_pose.position = Point(.5, .2, .7)
    goal_pose.orientation.z = 1  # rotate 180 degrees around z (see below)

    # the move_ptp service call requires a Pose argument
    resp = move_client.move_ptp(goal_pose)

    # not all poses can be reached, remember to check the result
    if not resp:
        rospy.logerr("move_ptp service call failed")

    time.sleep(2)

    # The orientation of a pose is stored as a quaternion and usually you
    # don't specify them manually. It's easier to e.g. use euler or RPY angles
    # HINT: display the coordinate systems in RViz to visualize orientations.
    # In RViz the XYZ axes are color coded in RGB: X=red, Y=green, Z=blue
    # the end effector is pointing along the blue z-axis

    # rotate the end effector 90 degrees around the (stationary) y axis
    goal_pose.orientation = orientation_from_rpy(0, pi / 2, 0)
    rospy.loginfo("move_ptp to same position but different orientation")
    move_client.move_ptp(goal_pose)  # (error check omitted)

    time.sleep(1)

    # rotate 90deg around the y-axis and 45deg around the new (relative) x-axis
    goal_pose.orientation = orientation_from_rpy(pi / 4, pi / 2, 0)
    rospy.loginfo("move_ptp to same position but different orientation")
    move_client.move_ptp(goal_pose)  # (error check omitted)

    time.sleep(2)

    return True


def main():
    rospy.init_node('hello_world')
    client = RLLDefaultMoveClient(hello_world)
    client.spin()


if __name__ == "__main__":
    main()

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
#include <geometry_msgs/Pose.h>
#include <ros/ros.h>

#include <rll_move_client/move_client_default.h>
#include <rll_move_client/util.h>

bool helloWorld(RLLDefaultMoveClient* const move_client)
{
  bool success;

  // moving by specifying joint angle values is not the most intuitive way
  // it's easier to specify the pose of the end effector we'd like to reach
  geometry_msgs::Pose goal_pose;
  goal_pose.position.x = .5;
  goal_pose.position.y = .2;
  goal_pose.position.z = .7;
  goal_pose.orientation.z = 1;  // rotate 180 degrees around z (see below)

  // the client.movePTP Request requires a Pose argument
  success = move_client->movePTP(goal_pose);

  // not all poses can be reached, remember to check the result
  if (!success)
  {
    ROS_ERROR("move_ptp service call failed");
  }

  ros::Duration(1).sleep();

  // The orientation of a pose is stored as a quaternion and its values usually
  // aren't specified manually. It's easier to use euler or RPY angles:
  // Here we rotate the end effector by 90degrees around the y axis
  // HINT: use the coordinate systems in RViz to better visualize rotations
  // in RViz the XYZ axes are color coded in RGB: X=red, Y=green, Z=blue
  // the end effector is pointing along the blue z-axis
  orientationFromRPY(0, M_PI / 2, 0, &goal_pose.orientation);

  // move to same position but different orientation!
  ROS_INFO("move_ptp to the same position but different orientation");
  move_client->movePTP(goal_pose);  // (error check omitted)

  ros::Duration(1).sleep();

  // rotate 90deg around the y-axis and 45deg around the x-axis
  orientationFromRPY(M_PI / 4, M_PI / 2, 0, &goal_pose.orientation);
  ROS_INFO("move_ptp to the same position but different orientation:");
  move_client->movePTP(goal_pose);

  ros::Duration(2).sleep();

  return true;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "hello_world");
  RLLCallbackMoveClient<RLLDefaultMoveClient> client(&helloWorld);
  client.spin();

  return 0;
}

Point based movement ist easier to understand. You only need to specify the position and orientation of the end effector and the robot will move there. However, you cannot control how the robot gets to this position.

The orientation part of a pose is stored as a quaternion. You usually don’t set the quaternion manually. Use the provided helper functions to generate the quaternion for you.

Linear movement

Python
C++
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
#! /usr/bin/env python
# -*- coding: utf-8 -*-

from __future__ import print_function

from math import pi
import time

import rospy
from geometry_msgs.msg import Pose, Point

from rll_move_client.client import RLLDefaultMoveClient
from rll_move_client.util import orientation_from_rpy


def hello_world(move_client):
    goal_pose = Pose()
    # Next up: move the end effector on a triangular path
    # while maintaining the same orientation
    rospy.loginfo("Next: move the end effector on a triangular path")

    # orient the z-axis "forward" (along the base x-axis)
    goal_pose.orientation = orientation_from_rpy(0, pi / 2, 0)

    # move to the starting position still in a ptp fashion
    goal_pose.position = Point(0.3, -0.6, 0.3)

    rospy.loginfo("move_ptp to the starting point of the triangle")
    move_client.move_ptp(goal_pose)  # (error check omitted)

    # move up, its a right angled triangle
    goal_pose.position.z = .7

    # this time we move on a linear trajectory to the specified pose
    rospy.loginfo("move_lin to the tip of the triangle")
    move_client.move_lin(goal_pose)  # (error check omitted)

    # next point is the upper right point of the triangle
    goal_pose.position.y = -0.25
    rospy.loginfo("move_lin to the upper right point of the triangle")
    move_client.move_lin(goal_pose)  # (error check omitted)

    # close the triangle by moving back diagonally to the start position
    goal_pose.position.y = -0.6
    goal_pose.position.z = .3
    rospy.loginfo("move_lin to the start to close the triangle shape")
    move_client.move_lin(goal_pose)  # (error check omitted)

    # Note: move_lin is not always successful, even if move_ptp succeeds.
    # This is because moving on a linear trajectory is more constraining
    # Example: move to a positive y-position will fail with move_lin
    goal_pose.position.y = 0.3
    rospy.loginfo("try to move_lin to: ")

    return True


def main():
    rospy.init_node('hello_world')
    client = RLLDefaultMoveClient(hello_world)
    client.spin()


if __name__ == "__main__":
    main()

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
#include <geometry_msgs/Pose.h>
#include <ros/ros.h>

#include <rll_move_client/move_client_default.h>
#include <rll_move_client/util.h>

bool helloWorld(RLLDefaultMoveClient* const move_client)
{
  geometry_msgs::Pose goal_pose;
  // Next up: move the end effector on a triangular path
  // while maintaining the same orientation
  ROS_INFO("Next: move the end effector on a triangular path");

  // orient the z-axis "forward" (along the base x-axis)
  orientationFromRPY(0, M_PI / 2, 0, &goal_pose.orientation);

  // move to the starting position still in a ptp fashion
  goal_pose.position.x = 0.3;
  goal_pose.position.y = -0.6;
  goal_pose.position.z = 0.3;

  ROS_INFO("move_ptp to the starting point of the triangle:");
  move_client->movePTP(goal_pose);

  // move up, its a right angled triangle
  goal_pose.position.z = .7;

  // this time we move on a linear trajectory to the specified pose
  ROS_INFO("moveLin to the tip of the triangle:");
  move_client->moveLin(goal_pose);

  // next point is the upper right point of the triangle
  goal_pose.position.y = -0.25;

  ROS_INFO("moveLin to the upper right point of the triangle:");
  move_client->moveLin(goal_pose);

  // close the triangle by moving back diagonally to the start position
  goal_pose.position.y = -0.6;
  goal_pose.position.z = .3;

  ROS_INFO("moveLin to the start to close the triangle shape:");
  move_client->moveLin(goal_pose);

  return true;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "hello_world");
  RLLCallbackMoveClient<RLLDefaultMoveClient> client(&helloWorld);
  client.spin();

  return 0;
}

Previously we moved in a point to point fashion to a desired pose. However, this has the drawback that we cannot guarantee how the end effector is reaching its new position. One alternative is to use Linear movement instead. This ensures that the end effector is moved on a linear trajectory to its target position. In the example above, three consecutive linear movements, forming a triangular path, are executed.

Python
C++
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
#! /usr/bin/env python
# -*- coding: utf-8 -*-

from __future__ import print_function

from math import pi
import time

import rospy
from geometry_msgs.msg import Pose, Point

from rll_move_client.client import RLLDefaultMoveClient
from rll_move_client.util import orientation_from_rpy


def hello_world(move_client):
    goal_pose = Pose()
    goal_pose.orientation = orientation_from_rpy(0, pi / 2, 0)
    goal_pose.position = Point(0.3, -0.6, 0.3)

    # Note: move_lin is not always successful, even if move_ptp succeeds.
    # This is because moving on a linear trajectory is more constraining
    # Example: move to a positive y-position will fail with move_lin
    goal_pose.position.y = 0.3
    rospy.loginfo("try to move_lin to: ")
    resp = move_client.move_lin(goal_pose)

    if not resp:
        rospy.logerr("move_lin service call failed (as expected)")

    # calling move_ptp with the exact same goal pose succeeds
    rospy.loginfo("try to move_ptp: ")
    resp = move_client.move_ptp(goal_pose)

    if not resp:
        rospy.logerr("move_ptp service call failed (unexpectedly)!")

    time.sleep(2)

    return True


def main():
    rospy.init_node('hello_world')
    client = RLLDefaultMoveClient(hello_world)
    client.spin()


if __name__ == "__main__":
    main()

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
#include <geometry_msgs/Pose.h>
#include <ros/ros.h>

#include <rll_move_client/move_client_default.h>
#include <rll_move_client/util.h>

bool helloWorld(RLLDefaultMoveClient* const move_client)
{
  bool success;
  geometry_msgs::Pose goal_pose;
  orientationFromRPY(0, M_PI / 2, 0, &goal_pose.orientation);
  goal_pose.position.x = 0.3;
  goal_pose.position.y = -0.6;
  goal_pose.position.z = 0.3;

  // note: client.moveLin is not always successful, even if client.movePTP succeeds.
  // This is because moving on a linear trajectory is more constraining
  // Example: move to a positive y-position will fail with client.moveLin
  goal_pose.position.y = 0.3;

  ROS_INFO("try to client.moveLin to:");
  success = move_client->moveLin(goal_pose);

  if (!success)
  {
    ROS_ERROR("moveLin service call failed (as expected)");
  }

  // calling movePTP with the exact same goal pose succeeds
  ROS_INFO("try to client.movePTP to:");
  success = move_client->movePTP(goal_pose);

  if (!success)
  {
    ROS_ERROR("move_ptp service call failed (unexpectedly)");
  }

  return true;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "hello_world");
  RLLCallbackMoveClient<RLLDefaultMoveClient> client(&helloWorld);
  client.spin();

  return 0;
}

Since move_lin() requires the end effector to travel on a linear trajectory, it is more constrained than move_ptp() which imposes no restrictions on how to reach the goal pose. It is therefore possible that move_lin() fails where a movement with move_ptp() succeeds, as illustrated in the example above.

Complete example

The complete code, which encompasses the examples above, is shown below:

Python
C++
Complete hello_world.py example
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
#! /usr/bin/env python
# -*- coding: utf-8 -*-

from __future__ import print_function

from math import pi
import time

import rospy
from geometry_msgs.msg import Pose, Point

from rll_move_client.client import RLLDefaultMoveClient
from rll_move_client.util import orientation_from_rpy


def hello_world(move_client):
    print("Hello world")  # avoid using print() for logging
    rospy.loginfo("Hello ROS")  # better use rospy.loginfo(), logerror()...

    # move to a random pose, this service call requires no arguments
    move_client.move_random()

    # The robot should now be moving (in RViz)! The delays in this code
    # are only for illustrative purposes and can be removed
    time.sleep(2)

    # move all seven joints into their zero position by calling the move_joints
    # service and passing the joint values as arguments
    rospy.loginfo("calling move_joints with all joint values = 0")
    move_client.move_joints(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)

    time.sleep(2)

    # rotate the fourth joint by 90 degrees (pi/2 since we work with radians)
    rospy.loginfo("calling move_joints with joint_4 = pi/2")
    resp = move_client.move_joints(0.0, 0.0, 0.0, pi / 2, 0.0, 0.0, 0.0)

    # previously we neglected to check the response of the service call.
    # You should always check the result of a service call. If a critical
    # failure occurs during the service call an exception will be raised.
    # If you do not check the result of service call you might miss a non
    # critical failure, because i.e. you passed an invalid pose
    if not resp:
        rospy.logwarn("move_joints service call failed (as expected)")

    # ups, moving the fourth joint by 90 degrees didn't work, we bumped into
    # the workspace boundaries
    # Let's try to move joint 2, 4 and 6 so that we end up in an upright
    # position of the end effector close to the front of the workspace.
    rospy.loginfo("calling move_joints to move into an upright position "
                  "close to the front of the workspace")
    resp = move_client.move_joints(0.0, pi / 4, 0.0, -pi / 4,
                                   0.0, -pi / 2, 0.0)

    if not resp:
        rospy.logerr("move_joints service call failed (unexpectedly)!")

    time.sleep(2)

    # moving by specifying joint angle values is not the most intuitive way
    # it's easier to specify the pose of the end effector we'd like to reach
    goal_pose = Pose()
    goal_pose.position = Point(.5, .2, .7)
    goal_pose.orientation.z = 1  # rotate 180 degrees around z (see below)

    # the move_ptp service call requires a Pose argument
    resp = move_client.move_ptp(goal_pose)

    # not all poses can be reached, remember to check the result
    if not resp:
        rospy.logerr("move_ptp service call failed")

    time.sleep(2)

    # The orientation of a pose is stored as a quaternion and usually you
    # don't specify them manually. It's easier to e.g. use euler or RPY angles
    # HINT: display the coordinate systems in RViz to visualize orientations.
    # In RViz the XYZ axes are color coded in RGB: X=red, Y=green, Z=blue
    # the end effector is pointing along the blue z-axis

    # rotate the end effector 90 degrees around the (stationary) y axis
    goal_pose.orientation = orientation_from_rpy(0, pi / 2, 0)
    rospy.loginfo("move_ptp to same position but different orientation")
    move_client.move_ptp(goal_pose)  # (error check omitted)

    time.sleep(1)

    # rotate 90deg around the y-axis and 45deg around the new (relative) x-axis
    goal_pose.orientation = orientation_from_rpy(pi / 4, pi / 2, 0)
    rospy.loginfo("move_ptp to same position but different orientation")
    move_client.move_ptp(goal_pose)  # (error check omitted)

    time.sleep(2)

    # You can also switch back to moving the joints directly. Let's repeat the
    # joint movement from before.
    move_client.move_joints(0.0, pi / 4, 0.0, -pi / 4,
                            0.0, -pi / 2, 0.0)

    # Next up: move the end effector on a triangular path
    # while maintaining the same orientation
    rospy.loginfo("Next: move the end effector on a triangular path")

    # orient the z-axis "forward" (along the base x-axis)
    goal_pose.orientation = orientation_from_rpy(0, pi / 2, 0)

    # move to the starting position still in a ptp fashion
    goal_pose.position = Point(0.3, -0.6, 0.3)

    rospy.loginfo("move_ptp to the starting point of the triangle")
    move_client.move_ptp(goal_pose)  # (error check omitted)

    # move up, its a right angled triangle
    goal_pose.position.z = .7

    # this time we move on a linear trajectory to the specified pose
    rospy.loginfo("move_lin to the tip of the triangle")
    move_client.move_lin(goal_pose)  # (error check omitted)

    # next point is the upper right point of the triangle
    goal_pose.position.y = -0.25
    rospy.loginfo("move_lin to the upper right point of the triangle")
    move_client.move_lin(goal_pose)  # (error check omitted)

    # close the triangle by moving back diagonally to the start position
    goal_pose.position.y = -0.6
    goal_pose.position.z = .3
    rospy.loginfo("move_lin to the start to close the triangle shape")
    move_client.move_lin(goal_pose)  # (error check omitted)

    # Note: move_lin is not always successful, even if move_ptp succeeds.
    # This is because moving on a linear trajectory is more constraining
    # Example: move to a positive y-position will fail with move_lin
    goal_pose.position.y = 0.3
    rospy.loginfo("try to move_lin to: ")
    resp = move_client.move_lin(goal_pose)

    if not resp:
        rospy.logerr("move_lin service call failed (as expected)")

    # calling move_ptp with the exact same goal pose succeeds
    rospy.loginfo("try to move_ptp: ")
    resp = move_client.move_ptp(goal_pose)

    if not resp:
        rospy.logerr("move_ptp service call failed (unexpectedly)!")

    time.sleep(2)

    # Besides moving the end effector, you can also change the arm angle to
    # move the elbow of the robot.
    goal_arm_angle = pi / 2
    move_client.move_lin_armangle(goal_pose, goal_arm_angle, True)
    # This is also possible with PTP movements.
    goal_arm_angle = -pi / 2
    goal_pose.position.z = .5
    move_client.move_ptp_armangle(goal_pose, goal_arm_angle)

    # the response sometimes holds more information than only a boolean status
    # move_random() returns the random pose the end effector moved to
    rospy.loginfo("move_random to a new random position")
    resp = move_client.move_random()  # (error check omitted)

    # we can obtain the chosen random pose from the response
    rospy.loginfo("move_random moved to: %s", resp)

    return True


def main():
    rospy.init_node('hello_world')
    client = RLLDefaultMoveClient(hello_world)
    client.spin()


if __name__ == "__main__":
    main()
Complete hello_world.cpp example
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
#include <geometry_msgs/Pose.h>
#include <ros/ros.h>

#include <rll_move_client/move_client_default.h>
#include <rll_move_client/util.h>

bool helloWorld(RLLDefaultMoveClient* const move_client)
{
  std::cout << "Hello World" << std::endl;  // avoid cout for logging
  ROS_INFO("Hello ROS");                    // better use ROS_INFO, ROS_ERROR...

  // move to a random pose, this service call requires no arguments
  move_client->moveRandom();

  // The robot should now be moving (in RViz)! The delays in this code
  // are only for illustrative purposes and can be removed
  ros::Duration(2).sleep();

  // move all seven joints into their zero (initial) position
  // set all joint values of the Request to zero (unnecessary, zero is the default)
  ROS_INFO("calling move_joints with all joint values = 0");
  move_client->moveJoints(0, 0, 0, 0, 0, 0, 0);

  ros::Duration(2).sleep();

  // rotate the fourth joint by 90 degrees (pi/2 since we work with radians)
  // the remaining joint values are still equal to zero
  ROS_INFO("calling move_joints with joint_4 = pi/2");
  bool success;
  success = move_client->moveJoints(0, 0, 0, M_PI / 2, 0, 0, 0);

  // previously we neglected to check the response of the service call.
  // You should always check the result of a service call!
  if (!success)
  {
    ROS_ERROR("move_joints service call failed (as expected)");
  }

  // ups, moving the fourth joint by 90 degrees didn't work, we bumped into
  // the workspace boundaries
  // Let's try to move joint 2, 4 and 6 so that we end up in an upright position
  // of the end effector close to the front of the workspace.
  ROS_INFO("calling move_joints to move into an upright position close to the front of the workspace");
  success = move_client->moveJoints(0, M_PI / 4, 0, -M_PI / 4, 0, -M_PI / 2, 0);

  if (!success)
  {
    ROS_ERROR("move_joints service call failed (unexpectedly)");
  }

  ros::Duration(2).sleep();

  // moving by specifying joint angle values is not the most intuitive way
  // it's easier to specify the pose of the end effector we'd like to reach
  geometry_msgs::Pose goal_pose;
  goal_pose.position.x = .5;
  goal_pose.position.y = .2;
  goal_pose.position.z = .7;
  goal_pose.orientation.z = 1;  // rotate 180 degrees around z (see below)

  // the client.movePTP Request requires a Pose argument
  success = move_client->movePTP(goal_pose);

  // not all poses can be reached, remember to check the result
  if (!success)
  {
    ROS_ERROR("move_ptp service call failed");
  }

  ros::Duration(1).sleep();

  // The orientation of a pose is stored as a quaternion and its values usually
  // aren't specified manually. It's easier to use euler or RPY angles:
  // Here we rotate the end effector by 90degrees around the y axis
  // HINT: use the coordinate systems in RViz to better visualize rotations
  // in RViz the XYZ axes are color coded in RGB: X=red, Y=green, Z=blue
  // the end effector is pointing along the blue z-axis
  orientationFromRPY(0, M_PI / 2, 0, &goal_pose.orientation);

  // move to same position but different orientation!
  ROS_INFO("move_ptp to the same position but different orientation");
  move_client->movePTP(goal_pose);  // (error check omitted)

  ros::Duration(1).sleep();

  // rotate 90deg around the y-axis and 45deg around the x-axis
  orientationFromRPY(M_PI / 4, M_PI / 2, 0, &goal_pose.orientation);
  ROS_INFO("move_ptp to the same position but different orientation:");
  move_client->movePTP(goal_pose);

  ros::Duration(2).sleep();

  // You can also switch back to moving the joints directly. Let's repeat the
  // joint movement from before.
  move_client->moveJoints(0, M_PI / 4, 0, -M_PI / 4, 0, -M_PI / 2, 0);

  // Next up: move the end effector on a triangular path
  // while maintaining the same orientation
  ROS_INFO("Next: move the end effector on a triangular path");

  // orient the z-axis "forward" (along the base x-axis)
  orientationFromRPY(0, M_PI / 2, 0, &goal_pose.orientation);

  // move to the starting position still in a ptp fashion
  goal_pose.position.x = 0.3;
  goal_pose.position.y = -0.6;
  goal_pose.position.z = 0.3;

  ROS_INFO("move_ptp to the starting point of the triangle:");
  move_client->movePTP(goal_pose);

  // move up, its a right angled triangle
  goal_pose.position.z = .7;

  // this time we move on a linear trajectory to the specified pose
  ROS_INFO("moveLin to the tip of the triangle:");
  move_client->moveLin(goal_pose);

  // next point is the upper right point of the triangle
  goal_pose.position.y = -0.25;

  ROS_INFO("moveLin to the upper right point of the triangle:");
  move_client->moveLin(goal_pose);

  // close the triangle by moving back diagonally to the start position
  goal_pose.position.y = -0.6;
  goal_pose.position.z = .3;

  ROS_INFO("moveLin to the start to close the triangle shape:");
  move_client->moveLin(goal_pose);

  // note: client.moveLin is not always successful, even if client.movePTP succeeds.
  // This is because moving on a linear trajectory is more constraining
  // Example: move to a positive y-position will fail with client.moveLin
  goal_pose.position.y = 0.3;

  ROS_INFO("try to client.moveLin to:");
  success = move_client->moveLin(goal_pose);

  if (!success)
  {
    ROS_ERROR("moveLin service call failed (as expected)");
  }

  // calling movePTP with the exact same goal pose succeeds
  ROS_INFO("try to client.movePTP to:");
  success = move_client->movePTP(goal_pose);

  if (!success)
  {
    ROS_ERROR("move_ptp service call failed (unexpectedly)");
  }

  ros::Duration(2).sleep();

  // Besides moving the end effector, you can also change the arm angle to
  // move the elbow of the robot.
  double goal_arm_angle = M_PI / 2;
  move_client->moveLinArmangle(goal_pose, goal_arm_angle, true);
  // This is also possible with PTP movements.
  goal_arm_angle = -M_PI / 2;
  goal_pose.position.z = .5;
  move_client->movePTPArmangle(goal_pose, goal_arm_angle);

  // the Response object sometimes holds more information than only success
  ROS_INFO("move_random to a new random position");
  move_client->moveRandom(&goal_pose);  // (error check omitted)

  // we can obtain the chosen random pose from the response
  ROS_INFO("move_random moved to: ");
  ROS_INFO_STREAM(goal_pose);

  return true;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "hello_world");
  RLLCallbackMoveClient<RLLDefaultMoveClient> client(&helloWorld);
  client.spin();

  return 0;
}