WEBVTT 00:00.080 --> 00:06.320 In this laboratory lesson, we are going to create a simple script, a simple C plus plus node that 00:06.320 --> 00:13.670 uses the movie two API to send a goal to a robot and move it to the desired position. 00:13.700 --> 00:20.090 So let's go back in Visual Studio Code, and since this is a simple script that you can use as a template 00:20.090 --> 00:27.170 for implementing your ideas with the movie C Plus Plus API, we will place it inside the Arduino bot 00:27.200 --> 00:28.730 C plus plus examples. 00:28.730 --> 00:39.380 So within the source folder here, let's create a new file called simple move it interface dot cpp. 00:39.410 --> 00:42.650 Here there is a typo interface. 00:42.650 --> 00:44.270 Perfect here. 00:44.270 --> 00:55.100 As always, let's start by include from the cl, cpp, the CL, cpp, Dehp that we are going to use 00:55.100 --> 01:02.530 in order to interact with all the Ros two functionalities from this C plus plus script and then as the 01:02.530 --> 01:08.860 node that we are going to create is very simple, we don't need to create a class, but we can directly 01:08.860 --> 01:09.940 insert everything. 01:09.940 --> 01:12.940 So all the code inside the main function. 01:12.940 --> 01:15.970 So let's create the function main. 01:21.550 --> 01:25.180 And here let's return zero. 01:25.210 --> 01:25.900 Perfect. 01:25.900 --> 01:31.960 And now in this function as usual let's start by initializing the Ros2 interface. 01:31.960 --> 01:38.410 So here let's use the rcl cpp init function. 01:38.410 --> 01:44.200 And let's provide the arguments of the main perfect. 01:44.200 --> 01:49.780 And then also we can create a new shared pointer so we can create a new node. 01:49.810 --> 01:53.830 And so first let's also include the memory package. 01:56.440 --> 02:03.450 And then let's use it here in order to create A shared pointer. 02:05.790 --> 02:08.730 To an object of type rcl. 02:08.760 --> 02:10.770 CPP node. 02:10.770 --> 02:11.310 Perfect. 02:11.310 --> 02:13.980 So we are creating a new Ros2 node. 02:13.980 --> 02:16.740 And let's simply call this one node. 02:16.770 --> 02:23.730 And we can initialize it with the Clcp node Makeshared. 02:23.730 --> 02:30.630 So with this function here we are initializing a new node and we need a function Makeshared. 02:30.630 --> 02:33.750 So here we need to provide the name of the node. 02:33.750 --> 02:41.070 So for example let's call it simple move it interface. 02:41.070 --> 02:42.120 Perfect. 02:42.120 --> 02:48.720 And now the only thing that actually we want to do in the main function is to execute another function 02:48.720 --> 02:50.670 that we are going to define soon. 02:50.970 --> 02:53.700 That is the move robot function. 02:53.700 --> 02:58.860 So basically this function here will be in charge of executing the movement of the robot. 02:58.860 --> 03:05.870 So of managing the Ros2 API for move it to and move the robot to the desired position. 03:05.870 --> 03:11.480 And this function here that we're going to define soon will also take as input the node. 03:11.480 --> 03:16.250 So this object here that we have created and after its execution. 03:16.250 --> 03:20.690 So after having moved the robot we can just shut down. 03:20.690 --> 03:24.290 So the clcp shut down. 03:24.320 --> 03:24.770 Perfect. 03:24.770 --> 03:28.130 So we can just shut down the Ros2 interface. 03:28.130 --> 03:29.030 Perfect. 03:29.030 --> 03:33.710 So the only thing that is missing now is to declare this function here. 03:33.710 --> 03:38.930 So the move robot function, let's define it. 03:38.930 --> 03:50.300 And this function will return void and will take as input a const shared pointer to an object of type 03:50.960 --> 03:52.640 clcp node. 03:52.640 --> 03:55.820 So it will take basically a node as input. 03:55.820 --> 03:58.100 That is this object here. 03:58.220 --> 03:59.090 Perfect. 03:59.120 --> 04:04.470 Now within this function we want to interact with the move it C plus plus Cplusplus API, and to do 04:04.470 --> 04:06.420 so we need to import another library. 04:07.560 --> 04:15.180 So let's include from the movie package and from the move group interface. 04:15.210 --> 04:20.280 Let's include the move group interface dot age perfect. 04:20.280 --> 04:22.890 And so now from this file here. 04:22.890 --> 04:28.710 So from this header file here we can create two new instances of the movie. 04:28.740 --> 04:39.750 So from the movie namespace and from this namespace from the planning interface we can create an instance 04:39.750 --> 04:45.120 of the move group interface class. 04:45.210 --> 04:51.090 And actually we need to create two instances of the move group interface class. 04:51.090 --> 04:53.340 And the first one. 04:53.340 --> 04:59.400 So this one here let's call it our move. 05:02.730 --> 05:03.720 Group. 05:04.010 --> 05:05.840 And the second one is that. 05:06.260 --> 05:11.300 So this one here, let's call it gripper. 05:11.480 --> 05:12.470 Move. 05:14.540 --> 05:15.530 Group. 05:16.220 --> 05:16.610 Perfect. 05:16.610 --> 05:20.630 So both of them are instances of the move group interface. 05:20.630 --> 05:22.460 And here this class here. 05:22.460 --> 05:27.530 So the move group interface class allows us to access and to send commands. 05:27.530 --> 05:32.900 And also to view the status of a specific move group in move it. 05:32.930 --> 05:40.280 If you remember, in fact when we configured move It for our robot, we defined two main move groups. 05:40.280 --> 05:46.640 That is one for the R, which we called simply R, and the other one for the gripper, which is that 05:46.640 --> 05:47.990 we called gripper. 05:47.990 --> 05:50.000 So now with this interface here. 05:50.000 --> 05:53.450 So with the move group interface class we can access. 05:53.450 --> 05:57.860 So separately the arm move group and the gripper move group. 05:57.860 --> 06:01.970 And so basically this means that we can send commands to the arm move group. 06:01.970 --> 06:08.530 And also to execute them Independently from the gripper move group, so we can move the arm in a way 06:08.530 --> 06:10.870 and then move the gripper in another way. 06:10.870 --> 06:15.550 And so we can plan trajectories for the two move groups independently. 06:15.670 --> 06:17.680 Now the constructor of this class. 06:17.680 --> 06:22.960 So the constructor of the move group interface class takes as input the node. 06:23.260 --> 06:27.940 So the instance that we have received here from the move robot function. 06:27.940 --> 06:33.520 And also here let's provide this node here that we have initialized here. 06:33.820 --> 06:38.710 Also the constructor of this class here takes a second input. 06:38.710 --> 06:41.980 That is the name of the move group that we want to take. 06:42.040 --> 06:48.250 And in this case so for the arm move group we want to access to the move group that we have configured 06:48.250 --> 06:49.900 and that we have called R. 06:50.080 --> 06:52.060 And for this second one. 06:52.060 --> 06:57.130 So for this second instance instead we want to access to the gripper move group. 06:57.130 --> 07:00.880 So to the move group that we have called gripper perfect. 07:00.910 --> 07:07.470 Now that we have the two move group interface objects we can use them to actually send goals to the 07:07.470 --> 07:08.100 robot. 07:08.100 --> 07:14.880 And in this case, in order to send a goal, we want to set the desired position of each of the joints 07:14.880 --> 07:15.690 of the robot. 07:15.690 --> 07:21.450 So we want to specify the desired position that each of the joints of the arm and of the gripper needs 07:21.450 --> 07:22.470 to reach. 07:22.620 --> 07:26.040 So this is expressed with two vectors. 07:27.240 --> 07:30.690 So let's create a vector of doubles. 07:30.720 --> 07:32.520 And let's also create another one. 07:32.520 --> 07:34.200 So we can copy this one. 07:35.940 --> 07:37.560 And we can paste it here. 07:37.560 --> 07:39.330 And let's call the first one. 07:39.330 --> 07:44.340 So this one our joint goal. 07:44.340 --> 07:48.690 So this will contain the desired position of all the joints of the arm. 07:48.690 --> 07:52.980 And for the moment let's just set this one with an empty list. 07:52.980 --> 07:59.160 And then let's also create the gripper joint goal. 07:59.160 --> 08:02.820 And also in this case let's initialize it with an empty list. 08:02.850 --> 08:04.530 Now for the arm. 08:04.530 --> 08:10.040 So here as you will certainly remember, the arm is composed of three joints. 08:10.040 --> 08:17.270 So the move group that we called arm is just composed of three joints that are the joint one, joint 08:17.300 --> 08:19.160 two and joint three. 08:19.190 --> 08:23.990 And for the goal for example, let's just set that we want the first joint. 08:23.990 --> 08:27.530 So the one of the base to rotate 90 degrees. 08:27.530 --> 08:32.300 And then we can just set all the other joints to zero. 08:32.300 --> 08:38.060 So the desired goal for the arm will just rotate the base of the arm. 08:38.060 --> 08:39.590 And for the gripper instead. 08:39.590 --> 08:44.240 The gripper is only composed of two joints, the joint four and the joint five. 08:44.240 --> 08:46.460 So the two fingers of the gripper. 08:46.460 --> 08:52.850 And also, as you might remember, the two grippers were not moved separately, but they will move together. 08:52.850 --> 08:55.850 So for example, let's open a little bit the gripper. 08:56.390 --> 09:02.810 And let's set the joint 4 to 0.7 and the joint 5 to 0.7. 09:02.810 --> 09:07.600 So as you can see they will move the same angle but in the opposite direction. 09:07.600 --> 09:13.090 So the overall behavior of the gripper will be that the two fingers will open. 09:13.240 --> 09:14.140 Perfect. 09:14.170 --> 09:20.140 Now that we have created the desired goal, so the vector with the desired position of the joints of 09:20.140 --> 09:21.670 the robot, we can move on. 09:21.670 --> 09:24.670 And so we can set this one to the robot. 09:25.000 --> 09:30.550 To do so, we can access to the R move group. 09:30.670 --> 09:37.870 And we can use the function set joint value target. 09:37.990 --> 09:43.570 And so we can also use the same function also on the gripper move group. 09:43.570 --> 09:50.080 So let's copy this one and let's paste it for the gripper move group. 09:50.200 --> 09:54.280 And so for the arm move group we want to set this one. 09:54.280 --> 09:57.670 So this vector here as the joint value target. 09:57.670 --> 09:59.380 So as the goal. 09:59.380 --> 10:02.290 And this one here instead is the vector. 10:02.290 --> 10:06.010 So is the values that we want to set for the gripper. 10:06.010 --> 10:09.060 So for the two joints that actuates the gripper. 10:09.090 --> 10:09.990 Perfect. 10:10.020 --> 10:11.490 Now this function here. 10:11.490 --> 10:18.570 So the set joint value target returns a boolean value indicating basically if the goal that we have 10:18.570 --> 10:18.930 set. 10:18.960 --> 10:25.170 So if this position here is actually reachable so is within the operational limits of the joint. 10:25.170 --> 10:31.560 So for example if we set this one too big or too small, it means that it's out of the mechanical limits 10:31.560 --> 10:32.670 of the robot. 10:32.700 --> 10:34.650 And so this function will fail. 10:34.650 --> 10:45.000 So let's store its result in a new Boolean variable that we can call r within bounds. 10:45.000 --> 10:48.870 And also as you can imagine we can copy this same instruction. 10:48.870 --> 10:53.790 And we can create a new variable called gripper within bounds. 10:53.790 --> 11:00.510 And so basically these two boolean variables we can use now in order to check whether the goal that 11:00.510 --> 11:03.150 we have set is actually reachable by the robot. 11:03.150 --> 11:21.100 So if the arm is not within bounds or the gripper is not within bounds. 11:21.250 --> 11:26.710 So if one of these two conditions is not verified, this means that we are not able to actually plan 11:26.710 --> 11:29.890 a path for the joint target. 11:29.890 --> 11:31.420 So for the goal that we have set. 11:31.450 --> 11:35.110 And so in this case we just want to print a warning message. 11:35.110 --> 11:38.080 So the goal is not achievable by the robot. 11:38.080 --> 11:42.820 So we can use the Clcp warn function. 11:42.820 --> 11:46.900 And then the Clcp get. 11:48.910 --> 11:50.320 Logger function. 11:50.320 --> 11:53.260 So let's get the Clcp logger. 11:53.260 --> 12:01.090 And here the message that we want to print is target joint position. 12:01.330 --> 12:07.330 Where outside of limits. 12:07.830 --> 12:15.030 And so let's just inform the user that the joint position are actually out of the limits and that this 12:15.030 --> 12:17.490 can incur in an error. 12:17.490 --> 12:20.760 And so let's just return. 12:20.760 --> 12:28.290 So let's just terminate the execution of this move robot as we are not able to reach this position at 12:28.290 --> 12:32.310 this point once we have verified the feasibility of the goal. 12:32.310 --> 12:40.050 So with this if statement, we can finally request move it to plan and execute the trajectory that brings 12:40.050 --> 12:42.660 our robot from the start position. 12:42.660 --> 12:47.460 So from the current position to the desired position that we have set here. 12:47.490 --> 12:48.450 Perfect. 12:48.450 --> 12:53.520 In order to do so, we need to create two instances of the plan class. 12:53.580 --> 13:07.500 So from the move it namespace and from the planning interface, let's access to the move group. 13:09.110 --> 13:10.520 Interface. 13:10.520 --> 13:13.880 And let's create a new instance of the plan class. 13:13.910 --> 13:18.770 Let's call the first one our plan perfect. 13:18.770 --> 13:21.500 And then we can also create a second instance. 13:21.680 --> 13:24.770 So we can copy this instruction here. 13:24.770 --> 13:26.330 And we can paste it here. 13:26.330 --> 13:28.940 And this one is for the gripper plan. 13:29.870 --> 13:32.630 So gripper plan. 13:32.780 --> 13:33.620 Perfect. 13:33.650 --> 13:39.440 Now to plan a trajectory that brings the robot from the current position to the desired one. 13:39.440 --> 13:41.090 It's very simple. 13:41.090 --> 13:43.760 And we just need to take this one here. 13:43.760 --> 13:45.800 So the arm move group object. 13:45.800 --> 13:48.770 And we can just call the function plan. 13:48.770 --> 13:54.800 So automatically this function here will plan a path from the current position of the robot to the desired 13:54.800 --> 13:57.020 one that we have set here. 13:57.110 --> 13:59.870 And also let's do the same for the gripper. 13:59.870 --> 14:05.330 So let's plan the trajectory for the gripper move group. 14:05.330 --> 14:08.600 And actually let's save the result of this plan. 14:08.600 --> 14:15.640 So let's save the result of this function here within the ARM plan variable. 14:15.640 --> 14:21.130 And for the gripper let's store it here within the gripper plan variable. 14:21.160 --> 14:23.890 Now we want to check whether this function here. 14:23.890 --> 14:27.160 So whether the plan function was executed successfully. 14:27.160 --> 14:32.620 So we want to check whether actually move it was able to plan a trajectory from the current position 14:32.620 --> 14:34.570 of the robot to the desired one. 14:34.570 --> 14:40.060 So we want to check that the result of this function is of type. 14:40.150 --> 14:42.670 Move it core. 14:45.190 --> 14:48.850 Move it error code. 14:48.850 --> 14:52.120 And this is of type success. 14:52.120 --> 14:53.560 So we want to check that. 14:53.560 --> 14:57.190 Actually the result of this plan is a success result. 14:57.190 --> 14:59.440 So we have a success error code. 14:59.440 --> 15:01.780 And now we want to save. 15:01.780 --> 15:03.940 So this equality here. 15:03.940 --> 15:07.630 So this control we want to save it into a new Boolean variable. 15:07.630 --> 15:09.750 And let's call the first one. 15:09.780 --> 15:13.530 Our plan success. 15:13.560 --> 15:20.280 And so this is the result of checking whether the result of the plan was actually this error code. 15:20.280 --> 15:22.200 So it was actually successful. 15:22.230 --> 15:24.690 And let's also do the same for the gripper. 15:24.690 --> 15:32.160 So let's create a new boolean variable called gripper plan success. 15:32.160 --> 15:36.090 And this contains the result of the planner. 15:36.090 --> 15:38.070 And so now we have these two variables. 15:38.070 --> 15:41.220 So the arm plan success and the gripper plan success. 15:41.220 --> 15:46.770 That will be set to true if the result of the planner is success. 15:47.220 --> 15:47.760 Perfect. 15:47.760 --> 15:53.760 So now we can use these two variables in order to check whether the planner was successfully executed. 15:53.760 --> 16:05.010 So if the R plan success and the gripper plan success. 16:05.010 --> 16:11.240 So if both of them succeeded, it means that actually we are ready to execute the plan that was calculated. 16:11.240 --> 16:16.160 So the plan that brings the robot from its current position to the desired one. 16:16.160 --> 16:23.960 And this is as simple as taking the arm move group and executing the move function. 16:23.960 --> 16:27.260 And we can do the same also for the gripper move group. 16:28.250 --> 16:30.650 So we can execute the move function. 16:30.650 --> 16:33.290 So basically automatically this function here. 16:33.290 --> 16:40.190 So the move function that we are calling on both the move groups will execute the plan that was calculated. 16:40.190 --> 16:45.590 So this one here for both the arm and the gripper otherwise. 16:45.770 --> 16:52.460 So if the planner so one of the two planners either the arm or the gripper was not successful. 16:52.460 --> 16:54.680 It means that we cannot actually execute. 16:54.680 --> 16:58.010 So we cannot actually move the robot in the desired position. 16:58.010 --> 17:04.670 And so in this case let's print an error message with rcl cpp error. 17:04.670 --> 17:15.100 And now let's use the rcl cpp get Logit function in order to access to the clcp logger. 17:15.100 --> 17:17.710 And then here let's print the message. 17:17.710 --> 17:22.030 One or more planners failed. 17:22.030 --> 17:27.730 So in order to inform the user that we cannot execute the trajectory because one of the planners failed. 17:27.730 --> 17:33.220 And so this is probably due to the fact that the start pose of the robot is invalid, or the target 17:33.220 --> 17:34.270 pose of the robot. 17:34.270 --> 17:36.370 So this one here is invalid. 17:36.370 --> 17:41.200 And in this case, let's just terminate the execution of the script. 17:41.380 --> 17:42.340 Perfect. 17:42.340 --> 17:44.110 So we can save this file. 17:44.110 --> 17:50.770 And with this we have completed the move robot function that interacts with the move it C plus plus 17:50.770 --> 17:51.490 API. 17:51.520 --> 17:58.960 In order to set a goal both for the arm and for the gripper, then plants a trajectory that brings the 17:58.960 --> 18:04.750 robot to the desired goal and then also executes the planned trajectory. 18:04.750 --> 18:05.500 Perfect. 18:05.500 --> 18:09.100 So now we are missing to install this script here. 18:09.100 --> 18:10.750 So to compile and install it. 18:10.790 --> 18:13.520 So in the file cmakelists.txt. 18:13.550 --> 18:24.440 Let's start by adding a new dependency that we've used and is the dependency from the move it Ros planning 18:25.940 --> 18:27.320 interface package. 18:27.320 --> 18:28.580 So this one here. 18:28.580 --> 18:31.400 And then we can copy these two instructions here. 18:31.400 --> 18:33.590 So the add executable instruction. 18:33.590 --> 18:38.990 And let's paste it here in order to install the new executable that we are creating. 18:38.990 --> 18:45.590 And this is the simple move it interface. 18:46.280 --> 18:47.120 Perfect. 18:47.120 --> 18:51.110 And this executable here is declared in the source folder. 18:51.110 --> 18:53.840 And in the simple move it interface dot cpp. 18:54.050 --> 19:01.760 Let's also add the dependencies for this script that are just the Clcp library and also this one here. 19:01.760 --> 19:04.310 So the move it Ros planning interface. 19:04.310 --> 19:06.260 So let's add it here. 19:06.260 --> 19:06.950 Perfect. 19:06.950 --> 19:12.400 And in fact as we can see from the C plus plus these are the only two dependencies that we have used 19:12.400 --> 19:13.750 for our script. 19:13.810 --> 19:21.700 Also, once we have compiled our node, let's also install it so we can add it here in the install instruction. 19:21.790 --> 19:26.110 And so let's install the simple movie interface executable. 19:26.110 --> 19:31.930 And then let's also add this executable here that we have compiled in the install instruction. 19:31.930 --> 19:36.040 So here so that it is installed and we can actually start using it. 19:36.040 --> 19:37.990 So let's save this file. 19:38.020 --> 19:43.390 Don't forget to add the missing dependencies also to the file package dot XML. 19:43.390 --> 19:47.950 So here we need to add a dependency from the package. 19:48.100 --> 19:54.580 Move it Ros planning interface. 19:54.580 --> 19:55.240 Perfect. 19:55.270 --> 20:00.040 Since we already have the Clcp dependency, let's save also this file. 20:00.040 --> 20:05.830 And now to conclude this lesson, let's just build the workspace in order to check that actually there 20:05.830 --> 20:06.940 are no errors. 20:06.940 --> 20:12.100 So let's go to the workspace and let's build it. 20:12.100 --> 20:18.990 So this will compile the new node that we've created within the Arduino bot CP examples. 20:18.990 --> 20:21.000 So now it is compiling it. 20:24.960 --> 20:27.930 As we can see here there is an error. 20:28.590 --> 20:32.520 And this is because there should be a typo somewhere. 20:32.520 --> 20:34.710 So let's open Visual Studio Code. 20:34.710 --> 20:37.440 And here we can see that actually there is a typo here. 20:37.440 --> 20:41.610 So this is gripper. 20:41.820 --> 20:44.310 So this is the name of this variable here. 20:44.310 --> 20:45.960 So gripper plan success. 20:46.140 --> 20:50.130 So let's save it and let's give it one more try. 20:56.760 --> 20:57.750 Here we go. 20:57.780 --> 21:01.590 Now the build was completed successfully so there are no more errors. 21:01.590 --> 21:05.280 And in the following laboratory lesson we are going to start this node. 21:05.280 --> 21:11.430 And we are going to see how it will plan a trajectory that moves the robot from the current state to 21:11.460 --> 21:13.050 the desired state.