In this laboratory lesson, we are going to create a simple script, a simple Python node that uses
the V2 APIs in order to send a goal to the robot and move it to a desired position.
Before following this lesson, please mind that the Python API for Moovit was introduced in Ros two
iron.
So if you are using an older Ros two version like for example Ros2 humble, it will not be available
and you can only interact with Moovit using the Cplusplus API that will be developed in the following
laboratory lesson.
So back in Visual Studio Code, let's start by creating our simple script in the Arduino Bot Pi examples
folder.
So this one here.
And so here within the Arduino Pi example sub folder, let's create a new file called simple Moovit
Interface dot pi.
Perfect.
So here, as always, let's start by importing the CLP library.
Perfect.
That is the interface that we can use in order to interact with the Ros2 functionalities.
And here, since we are implementing a very simple Ros2 node, there is no need actually to create a
new class that inherits from the node class, and instead we can directly instruct all the functions
and so all the code in the main function.
So let's define it.
So here let's execute the main function.
Perfect.
And now let's define it.
So let's define the function main in which.
Now we are going to insert all the code of our robot.
So of our API form of it.
Now here the first thing that we want to do is to initialize the Ros to interface with clp init.
Perfect.
And then also we want to insert all the code that will move the robot.
And so all the logic that interacts with the Moveit API for Python in a new function that we are going
to call move robot.
Perfect.
So this function here will actually be in charge of moving the robot from the current position to a
desired position that we will insert here.
And then after moving the robot, we want to simply shut down this node.
So let's use the CLP shutdown function.
Perfect.
So now we are missing to declare this function here.
So the move robot function in which we will actually insert all the logic for this script here.
So let's here let's define the move robot function.
Perfect.
And now in this move robot function we want to start interacting with the Moovit APIs for Python, and
to do so we can import from the Moovit package and from the planning library.
Let's import the class that is called Moovit Pi.
Perfect.
And so now we can start using this class to interact with the Moovit API.
And so here within the move robot function we can create a new instance of this class.
And let's call it Arduino bot.
So this is an instance of the movie Pi.
And we want.
So this one here under the hood is creating a new Ros2 node.
And let's call it.
So let's give it a name and let's call this node move it Pi.
Perfect.
And so here we have created a new instance of the movie Pi that we have called Arduino bot.
And now using this instance.
So using this object Here, we can access to the two move groups that we have defined in our moved configuration.
So if you remember from the previous lessons when we configured move It, we have defined two move groups,
one for the R so one that moves the R and one for the gripper.
So let's start interacting with the one of the r.
So let's call it Arduino bot arm.
Perfect.
And so from the Arduino bot object let's get a planning component.
So let's use this function here in order to access to the move group that we have called R.
Perfect.
And let's do the same also for the gripper.
So we can copy this line here and we can paste it.
And let's change this variable into Arduino bot gripper.
So with this one here we are accessing to the planning component.
So to the move group that we have called gripper Perfect.
And so with these two variables now we have accessed these two move groups.
Now with the Arduino robot arm and with the Arduino bot gripper variables, we can now access these
two groups.
So the arm and the gripper separately.
And so actually we can send a goal to each of them and then plan trajectories.
And also to execute the goal that we have planned independently.
So we can plan and execute a goal for the arm.
And then we can plan and execute another goal for the gripper to do so.
So to set a goal for the arm, for the gripper we need to import another class.
And this is from the movie package and from the core.
Let's import from the robot state the class that is called robot State.
So as the name of this class suggests, this will contain the state of the robot.
And so actually this can contain both the current state of the robot or also it can contain the desired
state of the robot.
Now let's create two new instances of this variable that is called our state.
And so this is an instance of the robot state.
And let's also create another one that is gripper state.
Again an instance of the robot state class.
And for the moment.
So within this variable we just want to contain.
So we just want to get the current state of the art.
So let's access to the Arduino bot and let's use the Get robot model function.
Perfect.
And also let's do the same here for the gripper.
So let's use the Arduino bot and then the get robot model function.
Perfect.
And now that we have the current state of the joints of the robot that we have created with the two
instances of this class glass here.
Now we can use them.
So we can use these two states to set a goal for the joints of the robot.
And in order to set the goal so we can access the arm state and we can use the set joint group positions
function.
So this function here and also for the gripper let's use the set joint group position perfect.
And for example so for the arm we want to set the position of the joints.
So the desired position for the move group that is called arm.
And we want to set it as a numpy array.
So first let's import the numpy library.
So let's import numpy as np.
And now here so where we are here in the set joint group position let's set the desired position.
So the goal position for the arm is a numpy array.
So here let's set the array.
And here we need to specify the desired position of all the joints that belong to the arm.
So if you remember the arm only has three joints.
So the joint of the base the shoulder and the elbow.
And for example let's just turn the base for 90 degrees.
So 1.57 radians.
And then we can instead set the other joints to zero.
So basically we are setting that the desired position of the arm will just rotate the base joint of
90 degrees.
And then let's also set the desired position for the gripper.
Perfect.
And also in this case this is a numpy array.
And as you remember there are only two joints that belongs to the gripper.
And also these two joints are not independent from each other but they are actuated Treated together.
And so let's for example open the gripper.
And so let's set that the joint number four will rotate of -0.7 radians.
And so the other one the joint number five will rotate the same angle.
So 0.7 but in the opposite direction.
Perfect.
So with this one we have set the goal state of the robot.
So the joints the desired position of the joints of the robot in the goal state.
Now before actually sending these goals to the planner.
So and to calculate the trajectory to these goals here, let's first reset the current state.
So we want first to set the current state of the robot before setting the desired state.
So again let's access to the Arduino robot arm.
And here let's use the function set start state to current State.
Perfect.
So the name of this function is quite self-explanatory.
And so basically we are setting the start state for this object here.
So for the Arduino both are to the current state.
So to the current position.
And so here we have set the start state.
Here we have the goal state.
So the desired goal state of the robot.
Now let's also do the same for the gripper.
So we can copy this instruction here.
And we can set the start state also for the gripper.
So also for the move group of the gripper.
And now we are just missing to set the goal state.
So to take the Arduino robot arm and we can use the set goal state function.
And we want to set the robot state.
And so actually here we can use this object here.
So the arm state object that we have created and in which we have set.
So this one here.
So this position of the joints.
Let's do the same also for the Arduino both gripper.
So here for the gripper let's set the goal state.
And also in this case we want to set the robot state that is this one here.
So the gripper state in which we have set the position.
So a numpy array for the desired configuration of the gripper.
Perfect.
So now we have that for the Arduino robot arm and for the Arduino bot gripper.
So for these two planning components we have set a start state.
We have also set a goal state.
Perfect.
So now we can just plan a trajectory from the start state to the goal state.
And this is pretty easy.
We just need to take the Arduino robot arm and just want to execute the function plan.
And also let's do the same for the Arduino bot gripper.
And let's use the function plan.
So this function here will just calculate a plan that brings the Arduino bot arm from the current position.
So this one here to the goal position.
So to this position here of the joints.
Now let's also save this plan that was generated into a new variable.
And let's call it for example our plan.
Result.
And also here let's call it gripper plan.
Result.
Perfect.
And now let's check whether the plan was successful.
So whether the planner was able to calculate a plan from the current position to the desired position.
And so if the arm plan result and the gripper plan result.
So if the planner was able to calculate a trajectory both for the arm and for the gripper.
Now we finally can execute it so we can execute the trajectory that was calculated.
And this is very simple.
So we can take this object here.
So the Arduino bot that is our robot and we can simply execute this one here.
So from the arm plan result we can take the trajectory.
So we want to execute this trajectory.
And let's also set the controllers to an empty list.
Just in order to use the default controllers that are loaded for the arm.
And let's execute also the trajectory for the gripper.
So still on the Arduino bot object.
So our robot let's execute this time the gripper plan result and also the trajectory.
And also in this case let's set the controllers as an empty array in order to use the default controllers.
otherwise.
So if one of the two planners failed.
So it means that we weren't able to calculate a path from the current state to the goal state.
And this, for example, can be due to the fact that the goal state is invalid.
So imagine if here we put an angle that is bigger than 90 degrees, so bigger than the mechanical limits
of the joints.
So of course this position is not reachable.
And so the planner will fail in this case.
Here we want to print an error message in the terminal.
So we can actually import from the RCL by logging the get logger function.
Perfect.
And so we can use this function here in the else statement.
So let's get the logger and let's get the RCL pi logger.
In order to print an error message that says here one or more planners failed perfect just to inform
the user that the goal state was not valid.
So we can save this script.
And with this our move robot function is completed and so we are ready to start it.
So we are ready to execute it.
But in order to do so we are going to need a new launch file.
So we are going to create a new launch file that actually starts this node here.
So let's do it again here within the Arduino bot Pi examples package.
And as by Ros2 conventions, we need to create a new folder called launch.
And within this folder we can create our launch file that we can call simple move it interface dot launch
dot pi.
Perfect.
And here before starting this node here.
So this Python node here the move it pi so the move it.
API for Python needs a move it configuration.
So basically they need a move it config builder.
And if you remember we have already used the moved config builder class here in the Arduino bot.
Move it launch file.
So in this launch file here, if we open it on one side we can see that here we have already used this
file here.
So the move it config builder.
Now also to start this one here.
So also to start this note that we have just developed.
We are going to need these same class here.
So actually we can copy it.
So we can copy all this function and we can paste it here.
Also we can import.
So here we can copy the include statement and we can paste it.
Perfect.
And now actually for our simple launch file we are not going to need these two arguments.
We are just going to need the moved it configuration perfect.
So we can close this one.
And here in this case the move it configuration basically is loading from the Arduino bot.
Move it.
And from the Arduino bot description all the Urdf model of the robot and also is loading all the configuration
files that we have created for move it.
Perfect.
So to this configuration file, in order to use the Moovit API, we need to add another configuration
so we can add it here.
After the trajectory execution configuration and the configuration that we are going to need is the
Moovit CP.
In fact under the hood basically the Moovit pi.
So this API here basically are just using the C plus plus API.
So we need to configure this parameter here.
So the Moovit cp with a new YAML file that you are going to create soon.
And this YAML file that we are going to create will be at the file path.
So still within the config folder.
And we are going to create it soon.
And we are going to call it planning Python API dot YAML.
And so as you can understand from the name, this YAML file will be just used for the Python API.
So to configure the Python API for move it.
So with this we have just created a configuration file.
So this one here an object of this type for our simple node.
So for the simple move with interface node.
So now we can actually start this node here.
So the simple move it interface node.
And so this is an instance of the node class.
And we want to start from the package that is called Arduino board by examples.
So from this package we want to start the executable that we have just created and it is called simple
move it.
Interface.
So this script here that we have just created and we want to configure it with some parameters.
Perfect.
And the parameters that we want to configure here.
So this is a list.
And simply in this list we want to use this one here.
So we want to use the move it config.
And we want to this one to be converted into a dictionary.
And apart from that we can also set the parameter.
So here let's use the parentheses.
And let's set the parameter.
Use same time to true as.
Basically we are going to use this node here.
So the simple move interface just with the simulated robot.
And so we can just hard code here the user same time to true.
Perfect.
So we are just missing now to return a launch description.
And the constructor of this class takes a list.
And within this list we need to insert all the nodes, all the actions that we want to execute with
this launch file.
And in this case this is just the simple move it interface.
So only this one here.
Also we can remove here all the classes that we are not using.
Perfect in order to have a clearer file and also a smaller one.
Perfect.
And so with this one we have finally started our node and we have also configured it.
And now we are just missing one last step that is this one here.
So we want to create this configuration file here.
So the planning Python API dot YAML.
That is a configuration file that is only used in order to configure the Python API.
So let's create it here within the Arduino bot move it package in the config folder.
So as we have said here and let's call this one as we have set this one here so we can copy the name.
And here let's create a new file and let's paste the name that we have inserted here.
So let's press enter.
And now in this file we just need to configure the planning pipelines that we want to use.
And actually the pipeline names.
So this is a vector containing all the planners that we want to use.
So for the moment we are fine just with Ompl.
So this is the only planner that we are going to use for this course.
Then let's also configure here the plan request params and let's set the parameter planar id.
And again we are only going to use om pl as a planar.
Then let's also set the parameter planning attempts.
And let's set this 1 to 1.
And then the planning.
Pipeline.
And this is again om PL.
So just the Ompl planner.
And then also we can set the max velocity scaling factor so we can set this 1 to 1.
So basically we are not scaling the maximum velocity.
And also we can set the max.
Acceleration scaling factor again to 1.0.
And these actually are the only parameters that we need in order to configure the Python interface.
So in order to configure the Python API.
So let's save also this file.
And now we are just missing to install everything.
So to install this script here.
So the simple movie interface within the Arduino Pi examples package.
And also to install the launch file that we have just created.
So we need to open the file setup.py that is in the Arduino dot pi examples.
And in order to install our node we can just copy this line here.
So the one that we have used for the Simple Action client and we can paste it here in order to install
the simple move it interface node.
And this node here is in the Arduino dot Pi examples folder.
And it's called Simple Movie Interface.
And still from this script here we want to execute the main function.
Then we also need to install the launch folder.
So this one here.
And in order to install the launch folder we need to modify this section here.
So the data files section.
So let's import some useful libraries that are the OS Python library.
And also from the glob library lets import the glob function.
Perfect.
And so now here so after the installation.
So as you can see here this instruction here will install the file package dot XML.
Now we also want to install the launch folder.
So here let's add a new instruction.
And from the US path join perfect.
And here from the share folder and from the package name variable, we want to install the launch folder
that we have just created.
Perfect.
Now here after the parenthesis we can use the glob function.
And again within this function we can use the OS path join in order to access the launch folder and
from the launch folder.
So from this folder we want to install everything that has the name launch and the extension.
So here let's use the parentheses p x y and also the extension y m a and also all other extensions.
Perfect.
So we can save also this file in which we have installed our simple movie interface and in which we
have also installed the launch directory.
So the launch file here and now don't forget as a last step to modify also the package dot XML as we
have introduced new dependencies.
In fact, we have introduced an execution dependency from the Ros2 launch package as we have created
a new launch file, and also we have a new execution dependency from the move it by package.
Anyway, this dependency here is only needed if you are using a newer version of Ros2.
So only if the condition is that your Ros distro.
So the Ros distribution that you are using is bigger than or equal to iron.
So for example, if you are using Ros two chassis you will have this package here.
So this library here that is that is not available in older version of Ros two like for example anvil
perfect.
So let's save also this file.
And in the following lesson after developing the same interface with the movie C plus plus API, we
are going to start this node.
And we are going to see how we can use it to plan a trajectory to the desired position of the robot
that we have set here in this file here.
So in these lines here.