Inverse kinematics tutorial
This tutorial will try to explain how to use the inverse kinematics functionality, while building a 7 DoF redundant manipulator. But before that, make sure to have a look at the various simple example scenes related to IK and FK in folder scenes/ik_fk_simple_examples. For this tutorial, we will build a non-dynamic manipulator, that just uses inverse kinematics without using any physics engine functionality. The V-REP CAD data related to this tutorial ("redundantManipulator.stl") is located in V-REP's installation folder's "cadFiles" folder. A V-REP scene related to this tutorial can be found in V-REP's installation folder's "tutorials\InverseKinematics" folder. Click [Menu bar --> File --> Import --> Mesh...] then select the file to import. Also refer to the section on how to import/export shapes. A dialog pops open asking about mesh scaling and mesh orientation. Click Ok. A single simple shape was imported and is located in the middle of the scene. The shape also appears in the scene hierarchy on the left hand side of the main window. Depending on how the original CAD data was exported, the imported CAD data could be at a different scale, different location, or even subdivided into several shapes. The assigned color of imported shapes is random. Following figure shows the imported shape:
As you can see, the import operation has left us with a single shape, where we expected several shapes. This means that we will have to divide the manipulator object ourselves: select the object (just click on it in the scene or the scene hierarchy), then click [Menu bar --> Edit --> Grouping/Merging --> Divide selected shapes]. Following is what you should have:
The original shape was divided into several sub-shapes (see also the scene hierarchy). The shape division algorithm operates by grouping all triangles that are linked by common edges. Depending on how the original mesh was created or exported, such a division procedure cannot be performed. In that case you will have to manually extract shapes in the triangle edit mode.
Next, we will change colors of the various objects so as to have a nice visual appearance. First double-click a shape icon in the scene hierarchy. The shape properties dialog opens. While a shape is selected, click on Adjust outside color in the dialog: this will allow you to adjust the various color components of the outside faces of the selected shape. For now, just adjust the ambient/diffuse color component of your shapes. To transfer the color of one shape to another shape, select both shapes and make sure the last selected shape (indicated with a white bounding box) is the one you want to take the color from, then simply click the Apply to selection button in the Colors section of the shape dialog. Feel free to adjust other visual parameters too, like the Shading angle parameter, the Edges width or the Edges color. Once you finished coloring, you might have following situation:
In next step, we will add the 7 joints of the manipulator. One way of doing this is to add the joints into the scene, then specify their appropriate position and orientation (through the position dialog and the orientation dialog). This is however not possible, when you don't know the exact joint positions as in our case, and so we will have to extract them from the shapes that we have:
Select all imported shapes and click [Menu bar --> Edit --> Bounding box alignment --> Align selected shapes' coordinate frame with world]. This operation guarantees that our bounding boxes are aligned with the absolute reference frame, and given the current manipulator configuration, represents the smallest bounding boxes. Click [Menu bar --> Add --> Joint --> Revolute] to insert a revolute joint into the scene. The default position is at (0;0;0) and its default orientation is vertical, and so the joint is hidden by the manipulator's base cylinder. While the joint is still selected, ctrl-select the base cylinder, then open the position dialog on the position tab and click the Apply to selection. This just positioned the joint at the exact same coordinates as the base cylinder (this operation however only slightly adjusted the joint's vertical position since it was already almost in position). Now repeat the procedure for all other joints in the manipulator (remember there should be a total of 7). All joints are in position now, however, some of them have the wrong orientation. Select all joints that should be aligned with the world's Y-axis, then enter (90,0,0) for the Alpha, Beta and Gamma items in the orientation dialog, on the orientation tab, then click the Apply to selection button. Next, select the joint that should be aligned with the world's X-axis, then enter (0,90,0) for Alpha, Beta and Gamma. All joints have the right position and orientation now.
You can now adjust the joint sizes (check the Joint length and Joint diameter items) in the joint properties dialog (that you can open by double-clicking a joint icon in the scene hierarchy). Make sure that all joints are clearly visible. This is what you should have:
The next step in this tutorial is to group shapes that belong to the same rigid entity. Select the 5 shapes that are part of link 1 (the base cylinder being "link 0"), then click [Menu bar --> Edit --> Grouping/Merging --> Group selected shapes]. Once the shapes are grouped in a compound shape, you could re-align its bounding box with the world, but this step is not required (and has only a visual effect). Repeat the same procedure with all shapes that logically belong together. In this tutorial we will not actuate the gripper's fingers, and so simply rigidly group them with the last link. When all shapes that are meant to be grouped share the same visual attributes, try merging them together instead ([Menu bar --> Edit --> Grouping/Merging --> Merge selected shapes]).
At this point you can rename all objects in the scene in following way, when going from base to tip: "redundantRobot" - "redundantRob_joint1" - "redundantRob_link1" - "redundantRob_joint2", etc. Just double-click an object's name in the scene hierarchy to edit its name.
Now we can build the kinematic chain, going from tip to base: select object "redundantRob_link7", then ctrl-select object "redundantRob_joint7" and click [Menu bar --> Edit --> Make last selected object parent]. Alternatively you can drag an object onto another one in the scene hierarchy to achieve a similar operation. Next do the same for object "redundantRob_joint7" and object "redundantRob_link6". Continue in a same way until the whole kinematic chain of the manipulator was built. This is what you should have (notice the scene hierarchy's structure):
Select all joints, then in the joint dialog, select "joint is in inverse kinematics mode" in the Joint mode section, then click Apply to selection. Then open the object common properties and in the Visibility layers section, disable layer 2 and enable layer 10, then click the related Apply to selection button. This just sent all joints to the visibility layer 10, effectively making them invisible. Have a look at the layer selection dialog if you wish to temporarily enable/disable some layers.
Now we will define an inverse kinematics task for the manipulator. In V-REP, an IK task requires specification of at least following elements:
We already have the "base" object (object "redundantRobot"). Let's add a dummy object, rename it to "redundantRob_tip" and set its position to (0.324,0,0.62) using the coordinate and transformation dialog. Next, attach the dummy to "redundantRob_link7" (select "redundantRob_tip", then "redundantRob_link7", then click [Menu bar --> Edit --> Make last selected object parent]. Our "tip" dummy is ready!
Now let's prepare the "target" dummy: copy and paste "redundantRob_tip" and rename the copy to "redundantRob_target". The "target" dummy is ready. Next, we have to inform V-REP that "redundantRob_tip" and "redundantRob_target" are a tip-target pair for inverse kinematics resolution. Double-click the dummy icon of "redundantRob_tip" in the scene hierarchy: this opens the dummy properties dialog. In the Dummy-dummy linking section, specify "redundantRob_target" as Linked dummy. Notice how both dummies got linked through a red stippled line in the scene hierarchy (the two dummies are also linked in the scene through a red line, but since both dummies are coincident, the line cannot be seen). In the same dialog, Link type is already IK, tip-target, which is the default value. This is what you should have by now:
At this stage, all elements for the definition of the inverse kinematics task are ready, and we just need to register the task as an IK group. Open the inverse kinematics dialog and click Add new IK group. A new item appears in the IK groups list: "IK_Group". While that item is selected, click Edit IK elements to open the IK element dialog. Next to the Add new IK element with tip button, select "redundantRob_tip" in the drop-down box, then click the Add new IK element with tip button. This just added an IK element that appears in the list. Further down, indicate "redundantRobot" as the Base. Finally, make sure that all items are checked in the Constraints section (check als Alpha-Beta and Gamma). Indeed, we want our "tip" dummy to follow our "target" dummy in position and orientation:
Close the IK element dialog. In the inverse kinematics dialog, you are free to check the item Mechanism is redundant, but at this stage, it won't make any difference since no joint limits or obstacle avoidance parameters have been defined.
Our inverse kinematics task is ready! Let's test it. Run the simulation, then select "redundantRob_target". Next, select the object translation toolbar button:
Now drag the object with the mouse: the manipulator should follow. Also try the object rotation toolbar button:
Try also holding down the ctr- or shift-keys during manipulation. Switch back to the object translation toolbar button, and try to drag the object as far as possible, and notice how the inverse kinematics task breaks. Indeed, this happens when a configuration is singular or not reachable, however there are workarounds to this behavior: while the simulation is still running, select "IK_Group" in the inverse kinematics dialog list, then specify DLS for the Calc. method item. Drag the object out of reach and notice how the inverse kinematics resolution became more stable. Try adjusting the Damping item up and down. Basically, when damping is large, resolution becomes more stable but slower. Practically, you can get the advantages of both resolution methods, all you need to do is define two identical "IK groups", where the first is not damped and the second is damped. Then, for the second "IK group", you can specify a conditional resolution (refer to the Edit conditional parameters item for more information). Now set back the Calc. method item to Pseudo inverse.
Now, in the IK element dialog, select "redundantRob_tip" then try to disable some of the Constraints items and notice how the manipulator behaves when the "redundantRob_target" object is dragged or rotated. Once you experimentated enough, reset all Constraints items to "checked", then stop the simulation.
What we will now do is add a way to easily manipulate the robot, without having to worry about breaking it by shifting the wrong objects around. We will therefore define it as a model. First, move "redundantRob_tip" and "redundantRob_target" to layer 11 to make both dummies invisible. Then shift-select all visible objects in the scene view, ctrl-click the object "redundantRobot" in the scene hierarchy to remove it from the selection, then open the object common properties dialog. Check the Select base of model instead item, then the related Apply to selection button. Clear the selection with <ESC>, then select "redundantRobot". In the same dialog, check the Object is model base item, then close the dialogs. Notice how a stippled bounding box now encompasses the whole manipulator:
Click any object on the manipulator and notice how the base dummy ("redundantRobot") always gets selected instead.
Next, let's add a "manipulation sphere" that we will use to manipulate the robot's gripper position/orientation. Click [Menu bar --> Add --> Primitive shape --> Sphere] to open the primitive shape dialog, indicate 0.05 for the X-Size, Y-Size and Z-Size, then uncheck the Create dynamic and respondable shape item and click OK. Adjust the newly added sphere's position to be the same as "redundantRob_target" (using the coordinate and transformation dialog). The sphere now appears at the tip of the manipulator. Rename the sphere to "redundantRob_manipSphere", then make it parent of "redundantRob_target". When you now run the simulation, you should be able to change the manipulator's configuration by moving the manipulation sphere around. Stop the simulation again.
Let's change a few other details. In the shape properties dialog, click Adjust outside color, then check the Opacity item. Notice how the sphere appearance changed. For a nicer appearance, check the Backface culling item in the shape dialog.
In the object common properties dialog uncheck all items in the Object special properties section (this is because the manipulation sphere doesn't really belong to the manipulator, it is more of a user-interface element). Now make "redundantRobot" parent of "redundantRob_manipSphere":
In the final step of this tutorial, we will register a collision object that should detect collisions between the manipulator and its environment. What we want is for every single shape (except for the manipulation sphere) in the manipulator to be able to detect a collision with the environment. Let's first define a collection for our manipulator:
Open the collection dialog with [Menu bar --> Tools --> Collections] or by clicking the appropriate toolbar button. Select "redundantRobot", then click Add new collection. A new, empty collection was added. We now need to define the collection content: click Add (make sure "redundantRobot" is still selected). Notice how the collection's content changed. Now select newly added collection item, then click Visualize selected collection: all objects composing the manipulator get a pink color in the scene! Rename the collection to "redundantRob". This is what you should have:
Now that the "redundantRob" collection is defined, we can register a collision object: open the collision dialog then click Add new collision object and specify the following item pair: "[Collection] redundantRob" - "all other collidable objects in the scene". This added a new collision object that you can rename in the list with a double-click (rename it to "redundantRob").
Collapse the "redundantRobot" tree in the scene hierarchy. Your redundant manipulator model is ready!
Run the simulation, and copy-paste a few times the manipulator. Shift/rotate the copies around, and change their configurations by dragging their manipulation spheres. Notice how every robot instance is fully functional, and how collisions are indicated with a color change. Open the inverse kinematics dialog, the collection dialog and the collision detection dialog. Notice how the listed items have also been automatically duplicated. Stop the simulation.
The procedure to register a minimum distance object is very similar to the collision object registration here above. All registered objects (collision detection, collections, IK groups, etc.) and all scene objects can be accessed through appropriate API calls. Additionally, they can be directly recorded and visualized by graph objects.