Kinematic Simulation
- Robot developers test the robot motions in simulation before executing them on real systems. Dynamic simulation provides realistic results, but uses much computation resource. Kinematic simulation just creates kinematic motions virtually where dynamics are not considered, but is much faster. This tutorial provides a guide of kinematic simulations of Motoman, Mikata Arm, etc.
Installation †
Refer to Installation to install ay_util. You also need to install robot descriptions (URDF; Unified Robot Description Format) of robots that you want to use (e.g. Motoman, Mikata Arm, UR).
Overview [Basic] †
ay_util provides a script named dummy_robot.py. This script creates a virtual robot consisting of N joints, maintains the state, and provides interface to move the robot.
How DummyRobot Works [Intermediate] †
- Loads joint names from the ROS parameter controller_joint_names.
- State (joint positions and velocities) starts at [0.0]*N, [0.0]*N where N is the number of joints.
- Publishes the joint state to the /joint_states topic.
- Provides the following topics to control the robot:
- /joint_path_command: Takes trajectory_msgs/JointTrajectory as a target trajectory, and generates the motion.
- /joint_speed_command: Takes trajectory_msgs/JointTrajectory as a target joint velocities. Note that only msg.points[0].velocities is used.
- /follow_joint_trajectory: actionlib.SimpleActionServer; this is also an interface to follow a target trajectory. Use actionlib.SimpleActionClient.
- The rate of topics and control can be specified by the argument. The default is 100 Hz.
Predefined Kinematic Simulations [Basic] †
Launch the simulation by:
Motoman SIA10F †
Launch the kinematic simulation by:
$ roslaunch ay_util motomansia10f_ksim.launch
In RViz,
- Add RobotModel to display the robot.
- Change "Global Options -> Fixed Frame" to "base".
Mikata Arm †
Original:
$ roslaunch ay_util mikata_ksim.launch
Rotated wrist:
$ roslaunch ay_util mikata_rot_ksim.launch
In RViz,
- Add RobotModel to display the robot.
- Change "Global Options -> Fixed Frame" to "base_link".
(Original)
(Rotated wrist)
Control the Robot from ay_trick †
You can test some motions from ay_trick with the same commands to the actual robots.
First, run CUITool:
$ rosrun ay_trick cui_tool.py
Second, setup ROS connection with the robot.py script.
Motoman kinemeatic simulation: > robot 'motos' Mikata kinemeatic simulation: > robot 'mikatas'
Then run the examples Scripts:
> q [0.0, 0.0, 0.0, 0.0] > moveq [0.2]*4, 2.0 Move to q: [0.2, 0.2, 0.2, 0.2] > x [0.18727699604285036, 0.037962926517793358, 0.15787542608757288, -0.029502791919169154, 0.29404383655184363, 0.095374505756769784, 0.95056378592206991] > movex ct.robot.FK()+np.array([0,0,+0.05,0,0,0,0]) Move to x: [ 0.187277 0.03796293 0.20787543 -0.02950279 0.29404384 0.09537451 0.95056379] > x [0.18727695395088745, 0.037962918027370909, 0.20787478810790036, -0.029502791950870695, 0.29404383654866284, 0.095374505859252237, 0.95056378591178747]
Brief explanation:
- q, x, moveq, movex are Scripts of ay_trick.
- They are actually stored in the ay_skill directory as q.py, x.py, etc.
- ct is a CoreTool object that has the robot object, the ROS connection, etc.
- ct.robot.FK() is the forward kinematics of the robot. Although it takes joint angles as a list, it is omitted. So, the current joint angles are used. As the result, it returns the current end-effector (wrist) pose (x,y,z and quaternion).
- You can also write the Python code. Numpy is imported as np; np.array([0,0,+0.05,0,0,0,0]) denotes an array of 7 elements.
- For example the following equation computes the end-effector (wrist) pose whose z-value is incre ct.robot.FK()+np.array([0,0,+0.05,0,0,0,0])
- Tips
- These examples are for Mikata Arm that has 4 DoF (degrees of freedom). So its joint angles returned by q has 4 elements, and the first parameter of moveq (target joint angles) is a list of 4 elements.
- Tips
- In this framework, the robots are controlled by the general robot model. You can use the same Scripts and functions of the robot object (ct.robot) for many robots such as Motoman. It also works with the simulated robots.
Further reading: