Step-by-step Robot Control with ay_trick
- In this tutorial, we learn robot control with the ay_trick framework. You will learn basic control of a robot including joint space control, task space control, trajectory control, and forward and inverse kinematics. We use Kinematic Simulation; i.e. you do not need to have a real robot.
Preparation †
- Complete the Installation of ay_tools.
- We use a kinematic simulation of Motoman.
- Complete the [Basic] level of Learning ay_trick.
- Complete the [Basic] level of Kinematic Simulation.
Using Basic Scripts to Operate Robot †
We use a Motoman robot model to learn basic Scripts (scripts provided in default). You can easily use other robots such as a Mikata Arm and Universal Robots. Note that the robot control class is designed to be common for the most of robots, i.e. the robot control class of Motoman and those of other robots have the same interface. You can use the same methods to operate robots, and the Scripts made for a robot may work with the other robots with a small modification. Thus, even if you want to learn about Universal Robots or Mikata Arm, this tutorial is still a good starting point.
Setup a Robot †
Launch a Kinematic Simulation and CUITool †
Let's launch the kinematic simulation:
$ roslaunch ay_util motomansia10f_ksim.launch
Configure rviz so that the robot is displayed.
Open another terminal, and run CUITool:
$ rosrun ay_trick cui_tool.py
Setup the Robot Object †
On CUITool, use the robot Script to setup the robot object:
trick or quit> robot 'motos' Setup robot for Motoman_SIM Created TContainer object 0x7fe33a506d10 ... Waiting for /state_validity_checker/get_allowed_collision_matrix... (t/o: 3.0) Motoman:trick or quit|A>
Explanation: The command robot is a Script ay_skill/robot.py that is a tool to setup the specified robot. More specifically, it creates an instance of a robot object, and stores it to the ct.robot member variable where ct is a CoreTool object. In this Script, ROS and other connections are done. If it has been finished correctly, you can play with the robot!
- Tips
- Each robot class is a subclass of TMultiArmRobot defined in ay_py/ros/robot.py, and the robot classes are defined in the same directory (ay_py/ros).
How to Use Other Robots? †
Refer to the page Available Robots.
Basic Scripts †
Here we will demonstrate the basic Scripts implemented in ay_skill. Each code demonstrated below is short. Reading them is a good practice to start writing your own Scripts.
To see the help (the functionality and the usage) of the Script, use the command with the -help option. For example,
> robot -help trick or quit> robot -help Robot initialization utility. Usage: robot ROBOT_NAME [, OPT1] Available ROBOT_NAME: 'NoRobot', 'pr2','PR2', 'pr2s','PR2_SIM', 'bx','Baxter', 'bxs','Baxter_SIM', 'bxn','BaxterN', ...
In the following, > denotes a caret and the succeeding characters are expected to be typed by you. # denotes a comment for explanation. Other messages are output. The behavior may be different due to the recent update of the library.
# Get the current joint angles (rad): > q [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Move to target joint angles ([0.2]*7 rad) in a specified duration (2.0 sec): > moveq [0.2]*7, 2.0 Move to q: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2] # You will see the robot moves in RViz. # Get the current wrist (base of end-effector) pose: > x [0.045849490702333226, -0.01119745922708924, 1.224457405286024, 0.038484205473739749, -0.092025751016609258, -0.0039395399851790188, 0.99500487792954539] # Move the end-effector pose to a target (0.1 m below the current pose) in 2.0 sec: > movex [0.045849490702333226, -0.01119745922708924, 1.124457405286024, 0.038484205473739749, -0.092025751016609258, -0.0039395399851790188, 0.99500487792954539], 2.0
Notes:
- The Motoman robot has 7 DoF (degrees of freedom), so the current joint angles and the target joint angles are lists of 7 elements.
- In CUITool, Python syntax is available, like [0.2]*7 (== [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2]).
- The duration of moveq and movex can be omitted. In that case, the default value 4 (sec) is used.
- The pose vector consists of position and orientation in quaternion.
- x = [px,py,pz, qx,qy,qz,qw]
- [px,py,pz]: Position (meters).
- [qx,qy,qz,qw]: Orientation in quaternion. Zero rotation is [0,0,0,1].
- The origin (coordinate frame) is the base frame of the robot. Each robot may have different base frame.
When we are using a robot with a gripper, we can use the following Scripts to get and control the gripper position:
# Get the current gripper position: > grip 0.01 # Move the gripper position to the specific target position: > grip 0.05
You will notice that the same Script grip.py is used to get and control the gripper position. This is possible by changing the Script behavior according to the number of arguments.
Writing Scripts †
We create Scripts of robot behaviors step-by-step. As written in Learning ay_trick, we will put the new Scripts in ay_tools/ay_skill_extra/YOUR_DIR/. YOUR_DIR is your directory, which can be any name. Maintaining YOUR_DIR with Git is recommended.
cf.
ay_tools/ +---ay_trick/ | +---ay_trick/ #ROS package of ay_trick | +---ay_skill/ #Directory to store Scripts +---ay_skill_extra/ #Directory to store extra (e.g. your) Scripts
If ay_skill_extra does not exist, create it. Here we assume A as YOUR_DIR. Since the directory is considered as a Python module, we need to put a blank file named __init__.py in the directory.
$ cd ros_ws/ay_tools/ $ mkdir -p ay_skill_extra/A/ $ cd ay_skill_extra/A/ $ touch __init__.py
Behavior Design Guide †
Since designing robotic behaviors needs imagination of robot movements, writing a huge script at once is difficult. CUITool provides an interactive interface with the robot, which is useful during designing the robot behaviors. The most of Python functions used in Scripts can be tested in the CUI beforehand. Testing individual functions and computations before unifying them as a whole scenario is a good strategy to make complicated behaviors.
Read the source code of the basic Scripts that we explored above, such as q.py, x.py, moveq.py, movex.py, and grip.py.
There are two major differences from usual Python scripts:
- We need to define Help and Run functions.
- The robot control is done through the CoreTool object ct which is given as an argument of the Run function.
Regarding Help and Run, refer to Learning ay_trick. The basic functions of the CoreTool object for robot control are introduced in the next section.
Basic Robot Functions †
Note that you can try all of the basic functions listed below in CUITool. Some parameters are omitted for the simplicity.
- ct.robot.DoF(arm=None): Get DoF of arm.
- ct.robot.BaseFrame: Property of the base frame.
- ct.robot.EndLink(arm=None): Get the end link (end-effector) frame of arm.
- ct.robot.Q(arm=None): Get joint positions of arm.
- ct.robot.FK(q=None, x_ext=None, arm=None): Get the pose of end-effector at joint angles q of arm (forward kinematics). x_ext is a local pose in the end link frame; FK computes a pose of x_ext in BaseFrame. If q is None, the current joint angles are used. If x_ext is None, it is considered as [0,0,0, 0,0,0,1].
- ct.robot.IK(x_trg, x_ext=None, start_angles=None, arm=None): Solving inverse kinematics of arm. x_trg is a target pose, x_ext is a local pose (see FK), and start_angles is a list of joint angles used as a seed of IK solver. If start_angles is None, the current joint angles are used.
- ct.robot.MoveToQ(q_trg, dt=4.0, arm=None, blocking=False): Move arm to a list of target joint angles in the duration dt. If blocking is True, this function blocks to return from the function until the robot finishes to move. If blocking is False, the function just sends a motion command to the robot and immediately returns.
- ct.robot.MoveToX(x_trg, dt=4.0, x_ext=None, arm=None, blocking=False): Move arm to a target end-effector pose x_trg.
- ct.robot.FollowQTraj(q_traj, t_traj, arm=None, blocking=False): Follow a joint angle trajectory. q_traj: joint angle trajectory [q0,...,qD]*N, and t_traj: corresponding times in seconds from start [t1,t2,...,tN].
- ct.robot.FollowXTraj(x_traj, t_traj, x_ext=None, arm=None, blocking=False): Follow an end-effector-pose trajectory.
- ct.robot.MoveToXI(x_trg, dt=4.0, x_ext=None, inum=30, arm=None, blocking=False): Control arm to the target end-effector pose with a linearly interpolated trajectory.
Note that arm denotes an index of the arm in above functions. When arm is None, the current arm is used in multi-arm robots. Single-arm robots like Motoman ignores the argument.
Examples of using these functions in CUITool:
> ct.robot.DoF() 7 > ct.robot.BaseFrame base_link > ct.robot.EndLink() link_t # Move the end-effector pose to 5 cm above the current pose in 2.0 sec: > movex ct.robot.FK()+np.array([0,0,+0.05,0,0,0,0]), 2.0 Move to x: [ 0.187277 0.03796293 0.20787543 -0.02950279 0.29404384 0.09537451 0.95056379]
Step-by-step Practice †
In the ay_skill_extra/A/ directory, create a file named first1.py and then edit it as follows:
#!/usr/bin/python
from core_tool import *
def Help():
return '''First script.
Usage: A.first1'''
def Run(ct,*args):
print len(args)
print args[0]
Note that you can use a template: ay_trick/ay_skill/template.py
Save the Script, and run it on CUITool:
> A.first1 -help First script. Usage: A.first1 > A.first1 10 1 10 > A.first1 10, 20, 30 3 10 > A.first1 0 Exception( <type 'exceptions.IndexError'> ) in CUI: IndexError('tuple index out of range',) ...
There was an error in the last execution, due to the invalid index access to args[0] since the number of elements was zero. This indicates that checking the number of arguments actually given to the Script is desirable for making better Scripts (e.g. print an error message when len(args) is not an expected value.
The above Script is just printing the arguments. Now we will modify it with robot control functions.
In the following, only the contents of the Run function is shown. Use any file name and edit Help as you like.
Increasing a Joint Angle †
Increasing 4th joint angle (q[3]) by 0.2 rad from the current value:
q= ct.robot.Q()
print q
q[3]+= 0.2
print q
ct.robot.MoveToQ(q, 2.0)
Shaking the End-effector †
A shaking motion where the robot repeats 4 times: moving to [+4cm,0,+4cm], and the current joint angles.
x= ct.robot.FK()
print x
for i in range(4):
x[0]+= 0.04
x[2]+= 0.04
ct.robot.MoveToX(x, 0.2, blocking=True)
x[0]-= 0.04
x[2]-= 0.04
ct.robot.MoveToX(x, 0.2, blocking=True)
ct.robot.MoveToX is called with blocking=True. If we do not specify this, the next ct.robot.MoveToX is executed before the previous one ends.
Following Joint Angle Trajectory †
We test ct.robot.FollowQTraj to follow a trajectory of joint angles.
First we define a trajectory of joint angles (q_traj) and a trajectory of times from the start (t_traj). Both of them are lists:
q_traj= [q_0, q_1, q_2, ...]
t_traj= [t_0, t_1, t_2, ...]
- q_i is a list of joint angles in radian whose length should be ct.robot.DoF().
- len(q_traj) == len(t_traj)
- t_i denotes a time from start.
- Ideally, q_i is reached at t_i seconds.
- t_i < t_{i+1}
In the following Script, we define a trajectory of joint angles starting at the current joint angles, and only the 3rd joint angle changes around the initial value.
q= ct.robot.Q()
q_traj= []
t_traj= []
q_traj.append(q)
t_traj.append(0.0)
for i in range(1,50):
t_traj.append(0.1*i)
q2= copy.deepcopy(q)
q2[2]+= 0.2*math.sin(3.0*0.1*i)
q_traj.append(q2)
print t_traj
print q_traj
ct.robot.FollowQTraj(q_traj, t_traj)
The reason why q2=copy.deepcopy(q) is used is that a normal copy q2=q just assigning a reference of q. If we modify the content of q2, q is modified accordingly. Using deepcopy reproduces the same object as q, i.e. the modification to q2 does not affect q.
Following End-Effector Pose Trajectory †
Similar to the above example, we test ct.robot.FollowXTraj to follow a trajectory of end-effector poses.
The idea is the same as FollowQTraj.
In the following Script, we try to control the robot to draw a circle with the end-effector.
r= 0.03
x= ct.robot.FK()
x1= copy.deepcopy(x)
x1[1]-= r
ct.robot.MoveToX(x1, 1.0, blocking=True)
x_traj= []
t_traj= []
for t in np.arange(0,2*math.pi,0.1):
t_traj.append(t)
x2= copy.deepcopy(x)
x2[1]+= -r*math.cos(1.0*t)
x2[2]+= r*math.sin(1.0*t)
x_traj.append(x2)
print t_traj
print x_traj
ct.robot.FollowXTraj(x_traj, t_traj)
The center of the circle is the current end-effector position. Its radius is defined by r. The above code modifies only (x2[1], x2[2]) that are y and z values. Thus this circle is on the YZ plain.