Mechanism_Model Tutorial
Background
The Mechanism_Model software package in CLARAty provides tools for modeling and analyzing connected articulated bodies. A XML file format for describing the configuration of a mechanism is defined. The Mechanism_Model software provides procedures for reading these XML files and computing kinematics information needed for control. The figure below illustrates the physical elements of a mechanism modeled in the software. Bodies are connected by Joints. A Body has only one parent Body but may have multiple child Bodies. At the root of the tree of Bodies is a Ground Body that represents the inertial reference frame. Joints may have multiple degrees of freedom. The nominal location of a Body, specified by its reference coordinate frame is defined with respect to the reference frame of its parent. Frames of interest on a body, for example where a sensor is mounted, are defined by Local coordinate frames with respect to the Bodies reference frame. The Joint connecting a Body to its parent articulates the Body to move it from its nominal location. Joint values specify the Joint articulation transformation.In addition to the kinematic information, the XML file format allows specification of inertial and volumetric and graphics data. Closed chains and Cartesian constraints are modeled as external constrained applied to the mechanism. The modeling approach is described in greater detail in the paper by Diaz, et.al. (see below).
The CLARAty classes used to implement the Mechanism_Model software package are described in the following section.
CLARAty Mechanism_Model Classes
| CLARAty Class |
Filename |
Module |
Brief Description |
| Mechanism_Model |
mechanism_model.h, mechanism_model.cc |
mechanism_model |
Mechanism_Model is the top-level object for the generic kinematics analysis package. The kinematics model is implemented as a tree-structure of ME_Body objects held in the Tree object member of Mechanism_Model. Member functions facilitate the creation/modification of the kinematic model and provide functions for performing kinematics analysis of the model. |
| ME_Body |
me_body_h, me_body.cc |
mechanism_model |
The ME_Body class implements a generic model of the rigid (extensible to non-rigid) elements of a kinematic structure. ME_Body contains a ME_Joint through which it connects to its parent ME_Body. ME_Body has physical/mechanical properties. It also has Frames that define coordinate frames of interest specified with respect to a reference frame on the body. |
| ME_Joint |
me_joint.h, me_joint.cc |
mechanism_model |
The ME_Joint object corresponds to the joint in a mechanism. ME_Joint attaches a ME_Body to its parent. It holds joint related data. |
| Frame |
frame.h, frame.cc |
mechanism_model |
A Frame is a labeled transform that is used to represent locations (position and orientation) of coordinate frames of interest on the body. It contains a Transform object, a name, frame type an index value and the name of the body it is attached to. |
| Mechanism_Model_IO |
mechanism_model_io.h, mechanism_model_io.c |
mechanism_model_io |
Mechanism_Model_IO is the I/O object for the the Mechanism_Model package. This class reads in data from XML files and puts the information into a Mechanism_Model object. It also writes data from a Mechanism_Model object to a XML file. |
| ME_Body_IO |
me_body.h, me_body.cc |
mechanism_model_io |
The ME_Body_IO object reads in body data from files and fills in a ME_Body object with the data. |
| ME_Joint_IO |
me_joint.h, me_joint.cc |
mechanism_model_io |
The ME_Joint_IO object reads in joint data from files and fills in a ME_Joint object with the data. |
| Frame_IO |
frame_io.h, frame_io.cc |
mechanism_model_io |
The Frame_IO class performs file I/O for the Frame class that is part of the Mechanism_Model package. |
| Transform_IO |
transform_io.h, transform_io.cc |
mechanism_model_io |
The Transform_IO class retrieves and manages transform data. Transform data may be specified as a position vector and one of many rotation data types. These include rotations about the X, Y or Z axes, three-angles as defined by the Euler rotations of fixed-axis rotations or D-H parameters. |
Using CLARAty Mechanism_Model Classes
A module called project_2dof_planar_arm was created to illustrate the use of the Mechanism_Model software for a two-degrees-of-freedom planar arm example. Two example files show how a Mechanism_Model model is created. A model can be created using manually entered lines of code as is illustrated in the demo_2dof_planar_arm.cc file. Alternatively, a model can be read in as is illustrated by the demo_2dof_planar_arm_xml.cc file from an XML model file. In the xml_model sub-directory in the module is the model data file named 2dof_planar_arm.xml. It models a 2 link planar arm with link lengths equal to 1.0 meters. The two joints of the arm rotate about the Z-axis. In its nominal configuration with its joints at 0 radians, the arm is aligned along the X-axis as illustrated below in the configuration closer to the viewer.
Module needed:
The XML model file corresponding to the model illustrated aboveis listed here:
<!-- This body isfirst link -->
<ME_Body name= "link1" >
<ME_Jointname = "joint1" type = "revolute" x_axis = "0"y_axis = "0" z_axis = "1">
</ME_Joint>
<Framename="ref1" type="reference">
<Transform>
<Position x="0" y="0" z="0" />
<Quaternion qi="0" qj="0" qk="0"qs="1" />
</Transform>
</Frame>
</ME_Body>
<!-- This body issecond link -->
<ME_Body name= "link2" parent = "link1" >
<ME_Jointname = "joint2" type = "revolute" x_axis = "0"y_axis = "0" z_axis = "1">
</ME_Joint>
<Framename="ref2" type="reference">
<Transform>
<Position x="1"y="0" z="0" />
<Quaternion qi="0" qj="0" qk="0"qs="1" />
</Transform>
</Frame>
<Framename="tip" type="local">
<Transform>
<Position x="1" y="0" z="0" />
<Quaternion qi="0.0" qj="0" qk="0"qs="1.0" />
</Transform>
</Frame>
</ME_Body>
</Mechanism_Model>
A new class, N_2DOF_Planar_Arm was derived from Mechanism_Model to demonstrate the implementation of a customized inverse kinematics algorithm. The class is found in the 2dof_planar_arm.h and 2dof_planar_arm.cc files in the project_2dof_planar_arm module. With these pieces in place, reading in a model file and performing the forward and inverse kinematics become trivial. Examples demonstrating these operations are found in the file demo_2dof_planar_arm_xml.cc.
To read in the 2dof planar arm model file and initialize parameters, we do the following:
To perform the forward kinematics for a given set of joint angles, we first set the arm joint angles, then query for the tip position:
arm.get_joint(1).set_value(0);
Transform tip_position =
arm.get_body("link2").get_frame("tip").get_absolute_transform();
To perform the inverse kinematics is just as easy. We first specify a desired tip location then query for the corresponding arm configuration.
N_2D_Point<double> tip(1.0,1.0);
arm.inverse_kinematics(tip,joint_values);
The 2-D vector of 2-D vectors, joint_values, contains the 2 sets of possible results.
References
A. Diaz-Calderon, I.A. Nesnas, W.S. Kim, and H. Nayar, "Towards a Unified Representation of Mechanisms for Robotic Control Software,"International Journal of Advanced Robotic Systems, Vol. 3, No. 1, pp. 061-066,2006.