claraty::Mechanism_Model

claraty::Mechanism_Model Class Reference
[Data Structure]

#include <mechanism_model.h>

Inheritance diagram for claraty::Mechanism_Model:

[legend]Collaboration diagram for claraty::Mechanism_Model:

[legend]List of all members.


Mechanism_Model model querying
bool	is_ground_body (ME_Body &body)
ME_Body &	get_ground_body ()
ME_Body &	get_body (unsigned int body_index)
ME_Body &	get_body (int body_index)
ME_Body &	get_body (Frame &frame)
ME_Body &	get_body (const std::string body_name)
ME_Body &	get_body (const std::string branch_head_name, const std::string body_name)
Frame &	get_frame (std::string frame_name)
Frame &	get_frame (int body_i, int frame_j)
ME_Joint &	get_joint (int i)
ME_Joint &	get_joint (std::string joint_name)
Transform	get_frame_transform (Frame &frame)
Transform	get_frame_transform (Frame &frame1, Frame &frame2)
Transform	get_body_absolute_transform (ME_Body &body)
bool	is_valid (std::string framename)
std::string &	get_version ()
std::string &	get_name ()
Tree< ME_Body > &	get_body_tree ()
void	set_name (std::string name)
void	set_version (std::string version)
void	set_joint_value (int joint_index, int dof_index, double value)
double	get_joint_value (int joint_index, int dof_index)
void	print_joint_values ()
unsigned int	_get_pre_order_body_index (Frame &body_node)
unsigned int	_get_pre_order_body_index (std::string body_name)
Public Member Functions
Mechanism_Model model creation and modification
void	add_body (ME_Body &body)
bool	add_body (ME_Body &body, std::string parent_body_name)
void	copy_descendents (Tree< ME_Body >::Iterator &to_tree_node, Tree< ME_Body >::Iterator &from_tree_node)
void	delete_all_bodies ()
bool	save (const std::string path_file_name)
void	rebuild_lists ()
Private Attributes
unsigned int	_number_of_bodies
unsigned int	_number_of_joints
unsigned int	_number_of_frames
std::vector< ME_Body * >	_body_vector
std::vector< ME_Joint * >	_joint_vector
std::vector< std::vector< Frame * > * >	_frame_vector_vector
std::vector< std::vector< double > >	_joint_values_table
Tree< ME_Body >	_body_tree
std::string	_name
std::string	_version
bool	_read_first_file
Semaphore	_mech_model_sem
Friends
std::ostream &	operator<< (std::ostream &os, Mechanism_Model &mm)

Detailed Description

Mechanism_Model class. The Mechanism_Model class contains a tree of ME_Bodies to represent a kinematic structure. Kinematic models are read in from XML data files that confirm to standard formats. An internal model of the kinematic structure is created from the model data. The Mechanism_Model class implements algorithms for performing kinematic operations on the model. These will include performing forward kinematics, computing the Jacobian and performing the inverse kinematics.

Definition at line 90 of file mechanism_model.h.

Constructor & Destructor Documentation

claraty::Mechanism_Model::Mechanism_Model ( )

Mechanism_Model class constructor. The Mechanism_Model class contains a tree of ME_Bodies to represent a kinematic structure. Kinematic models are read in from XML data files that confirm to standard formats. An internal model of the kinematic structure is created from the model data. The Mechanism_Model class implements algorithms for performing kinematic operations on the model. These will include perofrming forward kinematics, computing the Jacobian and performing the inverse kinematics.

Definition at line 50 of file mechanism_model.cc.

References _body_tree, _number_of_bodies, claraty::ME_Body::create_frame(), claraty::ME_Body::make_reference_frame(), and rebuild_lists().

00051 :_read_first_file(false)
00052 {
00053   // Create the "base" (ground) body and attach it to the tree at
00054   // the root position
00055   ME_Body * root_me_body = new ME_Body("ground", _body_tree);
00056   _body_tree.insert(_body_tree.begin_pre_order(), root_me_body);
00057   Frame & ground_ref_frame = root_me_body->create_frame();
00058   root_me_body->make_reference_frame(ground_ref_frame);
00059 
00060   _number_of_bodies = 1;
00061   rebuild_lists();
00062 }

Here is the call graph for this function:

claraty::Mechanism_Model::~Mechanism_Model ( ) [virtual]

Destructor for a Mechanism_Model object.

Definition at line 70 of file mechanism_model.cc.

References _body_vector, _frame_vector_vector, _joint_vector, _mech_model_sem, _number_of_bodies, claraty::Semaphore::acquire(), and claraty::Semaphore::release().

00071 {
00072   // Wait until mech_model is available and acquire it to lock out other access
00073   _mech_model_sem.acquire();
00074 
00075   _body_vector.clear();    // delete the vector of pointers to bodies
00076   _number_of_bodies = 0;
00077 
00078   _joint_vector.clear();           // delete the vector of pointers to joints
00079 
00080   _frame_vector_vector.clear(); // delete the vector of pointers to frames
00081 
00082   _mech_model_sem.release(); // Release the semaphore lock
00083 }

Here is the call graph for this function:

claraty::Mechanism_Model::Mechanism_Model ( )

Definition at line 50 of file mechanism_model.cc.

References _body_tree, _number_of_bodies, claraty::ME_Body::create_frame(), claraty::ME_Body::make_reference_frame(), and rebuild_lists().

00051 :_read_first_file(false)
00052 {
00053   // Create the "base" (ground) body and attach it to the tree at
00054   // the root position
00055   ME_Body * root_me_body = new ME_Body("ground", _body_tree);
00056   _body_tree.insert(_body_tree.begin_pre_order(), root_me_body);
00057   Frame & ground_ref_frame = root_me_body->create_frame();
00058   root_me_body->make_reference_frame(ground_ref_frame);
00059 
00060   _number_of_bodies = 1;
00061   rebuild_lists();
00062 }

Here is the call graph for this function:

claraty::Mechanism_Model::~Mechanism_Model ( ) [virtual]

Destructor for a Mechanism_Model object.

Definition at line 70 of file mechanism_model.cc.

References _body_vector, _frame_vector_vector, _joint_vector, _mech_model_sem, _number_of_bodies, claraty::Semaphore::acquire(), and claraty::Semaphore::release().

00071 {
00072   // Wait until mech_model is available and acquire it to lock out other access
00073   _mech_model_sem.acquire();
00074 
00075   _body_vector.clear();    // delete the vector of pointers to bodies
00076   _number_of_bodies = 0;
00077 
00078   _joint_vector.clear();           // delete the vector of pointers to joints
00079 
00080   _frame_vector_vector.clear(); // delete the vector of pointers to frames
00081 
00082   _mech_model_sem.release(); // Release the semaphore lock
00083 }

Here is the call graph for this function:

Member Function Documentation

void claraty::Mechanism_Model::add_body ( ME_Body & body )

Add a new body to the body tree.

Parameters:

[in] body Body to be added.

Returns:: True if successful, false if not.

Definition at line 92 of file mechanism_model.cc.

References _body_tree, _mech_model_sem, claraty::Semaphore::acquire(), rebuild_lists(), claraty::Semaphore::release(), and claraty::ME_Body::set_this_iterator().

Referenced by claraty::ME_Body::ME_Body().

00093 {
00094   _mech_model_sem.acquire();
00095   Tree<ME_Body>::Pre_Order_Iterator body_itr =
00096     _body_tree.append_child(_body_tree.begin_pre_order(), &body);
00097   body.set_this_iterator(body_itr);
00098   rebuild_lists();
00099   _mech_model_sem.release();
00100 }

Here is the call graph for this function:

bool claraty::Mechanism_Model::add_body	(	ME_Body &	body,
		std::string	parent_body_name
	)

Add a new body to the body tree. The body is a child of the parent_body.

Parameters:

`[in]`	body	Body to be added.
`[in]`	parent_body_name	Parent body to add to.

Returns:: True if successful, false if not

Definition at line 112 of file mechanism_model.cc.

References _body_tree, _mech_model_sem, claraty::Semaphore::acquire(), rebuild_lists(), claraty::Semaphore::release(), and claraty::ME_Body::set_this_iterator().

00114 {
00115   _mech_model_sem.acquire();
00116   bool attached_new_node = false;
00117 
00118   // First find the parent node in the tree
00119   Tree<ME_Body>::Pre_Order_Iterator p_me_body_node =
00120     _body_tree.begin_pre_order();
00121   Tree<ME_Body>::Pre_Order_Iterator p_end_me_body_node =
00122     _body_tree.end_pre_order();
00123   for (; p_me_body_node < p_end_me_body_node; ++p_me_body_node) {
00124     if (p_me_body_node->get_name() == parent_body_name) {
00125       // If found, attach to it
00126       Tree<ME_Body>::Pre_Order_Iterator body_itr =
00127         _body_tree.append_child(p_me_body_node, &body);
00128       body.set_this_iterator(body_itr);
00129       attached_new_node = true;
00130       break;
00131     }
00132   }
00133 
00134 
00135   rebuild_lists();
00136   _mech_model_sem.release();
00137   return attached_new_node;
00138 }

Here is the call graph for this function:

void claraty::Mechanism_Model::copy_descendents	(	Tree< ME_Body >::Iterator &	to_tree_node,
		Tree< ME_Body >::Iterator &	from_tree_node
	)

Copy the descendents of a node on a tree to be children of a node on a tree.

Parameters:

`[in]`	from_tree_node
`[out]`	to_tree_node

Definition at line 674 of file mechanism_model.cc.

References _body_tree, _mech_model_sem, claraty::Semaphore::acquire(), and claraty::Semaphore::release().

00677 {
00678   _mech_model_sem.acquire();
00679   _body_tree.copy_descendents(to_tree_node, from_tree_node);
00680 
00681   Tree<ME_Body>::Pre_Order_Iterator pre_iterate(_body_tree.begin_pre_order());
00682   Tree<ME_Body>::Pre_Order_Iterator end_iterate(_body_tree.end_pre_order());
00683   for (; pre_iterate < end_iterate; ++pre_iterate)
00684     {
00685       pre_iterate->set_mech_model_tree(_body_tree);
00686       pre_iterate->set_this_iterator(pre_iterate);
00687     }
00688   _mech_model_sem.release(); // Release the semaphore lock
00689   
00690 }

Here is the call graph for this function:

void claraty::Mechanism_Model::delete_all_bodies ( )

Delete the bodies in the tree without deleting the tree nodes

Definition at line 146 of file mechanism_model.cc.

References _body_tree.

00147 {
00148   Tree<ME_Body>::Post_Order_Iterator p_me_body_node =
00149     _body_tree.begin_post_order();
00150   Tree<ME_Body>::Post_Order_Iterator p_end_me_body_node =
00151     _body_tree.end_post_order();
00152   for (; p_me_body_node < p_end_me_body_node; ++p_me_body_node) {
00153     delete &(*p_me_body_node);
00154   }
00155 }

bool claraty::Mechanism_Model::save ( const std::string path_file_name )

Write model data in _body_tree to an XML file. This function is provided for de-bugging and model verification purposes. It write out data that was previously read in from an XML file using the load() function.

Parameters:

[in] path_file_name

Returns:: True if successful, false if unsuccessful.

Definition at line 170 of file mechanism_model.cc.

00171 {
00172   // Open the file
00173   ofstream saved_model(path_file_name.c_str(), ios::out);
00174 
00175   // Write the data
00176   saved_model << *this;
00177 
00178   // Close the file
00179   saved_model.close();
00180   
00181   return true;
00182 }

void claraty::Mechanism_Model::rebuild_lists ( )

Clear and fill in the _body_list, _joint_list and _node_list vectors of pointers to the ME_Body, ME_Joint and Frame objects in the tree. This is called after any modification to the model in Mechanism_Model.

Definition at line 191 of file mechanism_model.cc.

References _body_tree, _body_vector, _frame_vector_vector, _joint_values_table, _joint_vector, _number_of_bodies, _number_of_joints, claraty::ME_Joint::get_values_vector(), and claraty::ME_Body::set_this_iterator().

Referenced by add_body(), and Mechanism_Model().

00192 {
00193   ME_Body * me_body;
00194   ME_Joint * me_joint;
00195   vector<Frame *> * frame_vector;
00196   _body_vector.clear(); // clear the vector of pointers to bodies
00197   _number_of_bodies = 0;
00198 
00199   _joint_vector.clear();// clear the vector of pointers to joints
00200   _number_of_joints = 0;
00201 
00202   _frame_vector_vector.clear(); // clear the vector of pointers to
00203                                 // frames
00204 
00205   _joint_values_table.clear(); // clear the vector of joint value vectors
00206 
00207   Tree<ME_Body>::Pre_Order_Iterator begin_iterate(_body_tree.begin_pre_order());
00208   Tree<ME_Body>::Pre_Order_Iterator pre_iterate = begin_iterate;
00209   Tree<ME_Body>::Pre_Order_Iterator end_iterate(_body_tree.end_pre_order());
00210 
00211   // Iterate through the tree of bodies
00212   for (; pre_iterate < end_iterate; ++pre_iterate)
00213   {
00214         me_body = &(*pre_iterate);              // Save the pointer to
00215                                                 // the current body 
00216         me_body->set_this_iterator();
00217         _body_vector.push_back(me_body);        // to the body list vector
00218         ++_number_of_bodies;                    // increment the number of bodies
00219 
00220         if (pre_iterate > begin_iterate)        // the base body doesn't have a joint
00221         {
00222           me_joint = &(pre_iterate->get_joint()); // Save the pointer to the 
00223           _joint_vector.push_back(me_joint);      // current joint
00224           ++_number_of_joints;
00225           _joint_values_table.push_back(me_joint->get_values_vector());
00226           
00227         }
00228         frame_vector = &(pre_iterate->get_frame_vector());      // Save a pointer to the
00229                                                     //body's Vector of frames
00230         _frame_vector_vector.push_back(frame_vector);
00231   }
00232 }

Here is the call graph for this function:

bool claraty::Mechanism_Model::is_ground_body ( ME_Body & body )

ME_Body& claraty::Mechanism_Model::get_ground_body ( )

ME_Body & claraty::Mechanism_Model::get_body ( unsigned int body_index )

Returns:: Body by index

Definition at line 336 of file mechanism_model.cc.

References _body_tree, _mech_model_sem, claraty::Semaphore::acquire(), and claraty::Semaphore::release().

Referenced by get_body(), and claraty::N_2DOF_Planar_Arm::initialize().

00337 {
00338   _mech_model_sem.acquire();
00339   Tree<ME_Body>::Pre_Order_Iterator pre_iterate(_body_tree.begin_pre_order());
00340   pre_iterate += body_index;
00341   _mech_model_sem.release(); // Release the semaphore lock
00342   return *pre_iterate;
00343 }

Here is the call graph for this function:

ME_Body& claraty::Mechanism_Model::get_body ( int body_index ) [inline]

Definition at line 117 of file mechanism_model.h.

References get_body().

00117                                            {
00118     return get_body((unsigned int) body_index);};

Here is the call graph for this function:

ME_Body& claraty::Mechanism_Model::get_body ( Frame & frame ) [inline]

Definition at line 119 of file mechanism_model.h.

References claraty::Frame::get_body().

00119 {return *frame.get_body();}

Here is the call graph for this function:

ME_Body & claraty::Mechanism_Model::get_body ( const std::string body_name )

Get the ME_Body in the body tree for a given body name NOTE: If there are multiple bodies on the tree with common names, the first pre-ordered node is returned.

Parameters:

[in] body_name The name of the body.

Returns:: A reference to the body in the tree. Iterator points to end if body is not found

Definition at line 271 of file mechanism_model.cc.

References _body_tree, _mech_model_sem, claraty::Semaphore::acquire(), and claraty::Semaphore::release().

00272 {
00273   // Wait until mech_model is available and acquire it to lock out other access
00274   _mech_model_sem.acquire();
00275   
00276   Tree<ME_Body>::Pre_Order_Iterator pre_iterate(_body_tree.begin_pre_order());
00277   if (body_name.length() > 0)
00278     {
00279       Tree<ME_Body>::Pre_Order_Iterator end_iterate(_body_tree.end_pre_order());
00280       for (; pre_iterate < end_iterate; ++pre_iterate)
00281         {
00282           if (pre_iterate->get_name() == body_name)
00283             break;
00284         }
00285     }
00286   _mech_model_sem.release(); // Release the semaphore lock
00287   return *pre_iterate;
00288 }

Here is the call graph for this function:

ME_Body & claraty::Mechanism_Model::get_body	(	const std::string	branch_head_name,
		const std::string	body_name
	)

Get the ME_Body in the body tree that has branch_head at its head for a given body name NOTE: If there are multiple bodies on the branch with common names, the first pre-ordered node is returned.

Parameters:

`[in]`	branch_head_name	The name of the body at the branch head.
`[in]`	body_name	The name of the body.

Returns:: an Iterator pointing to the body node in the tree Iterator points to end if body is not found

Definition at line 305 of file mechanism_model.cc.

References _body_tree, _get_pre_order_body_index(), _mech_model_sem, claraty::Semaphore::acquire(), and claraty::Semaphore::release().

00307 {
00308   // Wait until mech_model is available and acquire it to lock out other access
00309   _mech_model_sem.acquire();
00310   Tree<ME_Body>::Pre_Order_Iterator branch_head = _body_tree.begin_pre_order();
00311   branch_head += _get_pre_order_body_index(branch_head_name);
00312   Tree<ME_Body>::Pre_Order_Iterator pre_iterate(branch_head);
00313   Tree<ME_Body>::Pre_Order_Iterator end_iterate(_body_tree.end_pre_order());
00314   for (; pre_iterate < end_iterate; ++pre_iterate)
00315     {
00316       if (pre_iterate->get_name() == body_name)
00317         break;
00318       if (!pre_iterate.is_descendent(branch_head))
00319         {
00320           pre_iterate = end_iterate;
00321           break;
00322         }
00323     }
00324   _mech_model_sem.release();
00325   return *pre_iterate;
00326 }

Here is the call graph for this function:

Frame & claraty::Mechanism_Model::get_frame ( std::string frame_name )

Get the first (pre-ordered) frame in the tree with the given name

Parameters:

[in] frame_name

Returns:: Reference to the frame

Definition at line 436 of file mechanism_model.cc.

References _frame_vector_vector, and get_name().

00437 {
00438   unsigned int index_vec_frame_vec;
00439   unsigned int index_frame_vec;
00440   bool found = false;
00441   
00442   for (index_vec_frame_vec = 0; index_vec_frame_vec
00443          < _frame_vector_vector.size(); ++index_vec_frame_vec)
00444     {
00445       for (index_frame_vec = 0; index_frame_vec <
00446              (_frame_vector_vector[index_vec_frame_vec])->size();
00447            ++index_frame_vec)
00448         {
00449           if ((*(*(_frame_vector_vector[index_vec_frame_vec]))
00450                 [index_frame_vec]).get_name() == frame_name)
00451             {
00452               found = true;
00453               break;
00454             }
00455         }
00456       if (found)
00457         break;
00458       
00459     }
00460   return (*(*(_frame_vector_vector[index_vec_frame_vec]))[index_frame_vec]);
00461 }

Here is the call graph for this function:

Frame & claraty::Mechanism_Model::get_frame	(	int	body_i,
		int	frame_j
	)

Returns:: Frame by index

Definition at line 366 of file mechanism_model.cc.

References _frame_vector_vector.

00367 {
00368   return *(*(_frame_vector_vector[body_i]))[frame_j];
00369 }

ME_Joint & claraty::Mechanism_Model::get_joint ( int i )

Returns:: Joint by index

Definition at line 353 of file mechanism_model.cc.

References _joint_vector.

Referenced by get_joint_value(), and set_joint_value().

00354 {
00355   return *_joint_vector[i];
00356 }

ME_Joint & claraty::Mechanism_Model::get_joint ( std::string joint_name )

Get the first (pre-ordered) joint in the tree with the given name

Parameters:

[in] joint_name

Returns:: Reference to the joint.

Definition at line 472 of file mechanism_model.cc.

References _joint_vector, and get_name().

00473 {
00474   unsigned int index;
00475   
00476   for (index = 0; index < _joint_vector.size(); ++index)
00477     {
00478       if (_joint_vector[index]->get_name() == joint_name)
00479         break;
00480     }
00481   return *(_joint_vector[index]);
00482 }

Here is the call graph for this function:

Transform claraty::Mechanism_Model::get_frame_transform ( Frame & frame )

Get the transform associated with the given frame.

Parameters:

[in] frame Frame to be queried for.

Returns:: Transform of frame absolite position.

Definition at line 380 of file mechanism_model.cc.

References claraty::Frame::get_absolute_transform().

00381 {
00382   return frame.get_absolute_transform();
00383 }

Here is the call graph for this function:

Transform claraty::Mechanism_Model::get_frame_transform	(	Frame &	frame1,
		Frame &	frame2
	)

Get the relative transform between frame1 and frame2.

Parameters:

`[in]`	frame1
`[in]`	frame2

Returns:: Transform of frame absolute position

Definition at line 395 of file mechanism_model.cc.

References claraty::Frame::get_relative_transform().

00396 {
00397   return frame1.get_relative_transform(frame2);
00398 }

Here is the call graph for this function:

Transform claraty::Mechanism_Model::get_body_absolute_transform ( ME_Body & body )

Get the body transform withrespect to the ground frame.

Parameters:

[in] body

Returns:: Transform of frame absolute position

Definition at line 409 of file mechanism_model.cc.

References _body_tree, claraty::ME_Body::get_this_iterator(), and claraty::Trans< T, Rotation_Type >::identity.

00410 {
00411   Tree<ME_Body>::Chain_Iterator body_chain_node(_body_tree.begin_pre_order(),
00412                                            body.get_this_iterator());
00413   body_chain_node.set_begin();
00414   
00415 
00416   Transform result = Transform::identity;
00417 
00418   for (; body_chain_node < _body_tree.end_chain(body_chain_node); ++body_chain_node)
00419     {
00420       result = result
00421         * body_chain_node->get_reference_frame().get_transform()
00422         * body_chain_node->get_joint().get_transform();
00423     }
00424   return result;
00425 
00426 }

Here is the call graph for this function:

bool claraty::Mechanism_Model::is_valid ( std::string node_name )

True if nodename is the name of a body on the on the body tree, false if not

Parameters:

[in] node_name

Returns:: True if node_name is the name of a body on the body tree, false if not.

Definition at line 561 of file mechanism_model.cc.

References _body_tree, _mech_model_sem, claraty::Semaphore::acquire(), and claraty::Semaphore::release().

00562 {
00563   bool retVal = false;
00564   _mech_model_sem.acquire();
00565   Tree<ME_Body>::Pre_Order_Iterator pre_iterate(_body_tree.begin_pre_order());
00566   Tree<ME_Body>::Pre_Order_Iterator end_iterate(_body_tree.end_pre_order());
00567   for (; pre_iterate < end_iterate; ++pre_iterate)
00568     {
00569       if (pre_iterate->get_name() == node_name)
00570         {
00571           retVal = true;
00572           break;
00573         }
00574     }
00575   _mech_model_sem.release(); // Release the semaphore lock
00576   return retVal;
00577 }

Here is the call graph for this function:

std::string & claraty::Mechanism_Model::get_version ( )

Return the version number of the mechanism model

Returns:: The version number of the mechanism model.

Definition at line 600 of file mechanism_model.cc.

References _version.

Referenced by claraty::operator<<().

00601 {
00602   return _version;
00603 }

std::string & claraty::Mechanism_Model::get_name ( )

Return the name of the mechanism model

Returns:: The name of the mechanism model.

Definition at line 587 of file mechanism_model.cc.

References _name.

Referenced by get_frame(), get_joint(), claraty::operator<<(), and print_joint_values().

00588 {
00589   return _name;
00590 }

Tree< ME_Body > & claraty::Mechanism_Model::get_body_tree ( )

Return a reference to the body tree.

Returns:: Index of the body.

Definition at line 546 of file mechanism_model.cc.

References _body_tree.

Referenced by claraty::Wheel_Locomotor_Model::map_mechanism_model_to_wheel_locomotor_model(), and claraty::operator<<().

00547 {
00548   return _body_tree;
00549 }

void claraty::Mechanism_Model::set_name ( std::string name )

Set the name of the mechanism model

Parameters:

[in] name

Definition at line 613 of file mechanism_model.cc.

References _name.

00614 {
00615   _name = name;
00616 }

void claraty::Mechanism_Model::set_version ( std::string version )

Set the version number of the mechanism model

Parameters:

[in] version

Definition at line 659 of file mechanism_model.cc.

References _version.

00660 {
00661   _version = version;
00662 }

void claraty::Mechanism_Model::set_joint_value	(	int	joint_index,
		int	dof_index,
		double	value
	)

Set the value of a joint dof

Parameters:

`[in]`	joint_index
`[in]`	dof_index
`[in]`	value

Definition at line 628 of file mechanism_model.cc.

References get_joint(), and claraty::ME_Joint::set_value().

00630 {
00631   get_joint(joint_index).set_value(value, dof_index);
00632   //  (_joint_values_table[joint_index])[dof_index] = value;
00633 }

Here is the call graph for this function:

double claraty::Mechanism_Model::get_joint_value	(	int	joint_index,
		int	dof_index
	)

Get the value of a joint dof

Parameters:

`[in]`	joint_index
`[in]`	dof_index

Definition at line 644 of file mechanism_model.cc.

References get_joint(), and claraty::ME_Joint::get_value().

00646 {
00647   return get_joint(joint_index).get_value(dof_index);
00648   //  return (_joint_values_table[joint_index])[dof_index];
00649 }

Here is the call graph for this function:

void claraty::Mechanism_Model::print_joint_values ( )

Print to screen the joint values for each joint. This is primarily to be used for debugging.

Definition at line 241 of file mechanism_model.cc.

References _joint_values_table, _joint_vector, _number_of_joints, and get_name().

00242 {
00243   unsigned int i;
00244   int j;
00245 
00246   cout << "Number of joints = " << _number_of_joints << endl;
00247   for (i=0; i<_number_of_joints; ++i)
00248     {
00249       cout << "Joint " << i << ": " << _joint_vector[i]->get_name() << " ";
00250       for (j=0; j<_joint_vector[i]->get_number_dofs(); ++j)
00251         {
00252           cout << (_joint_values_table[i])[j] << "   " 
00253                << (_joint_vector[i]->get_values_vector())[j] << endl;
00254         }
00255       cout << endl;
00256     }
00257 }

Here is the call graph for this function:

unsigned int claraty::Mechanism_Model::_get_pre_order_body_index ( Frame & body_node ) [private]

Get the index of a body

Parameters:

[in] body_node A node on the body.

Returns:: Index of the body.

Definition at line 493 of file mechanism_model.cc.

References _body_tree, _mech_model_sem, claraty::Semaphore::acquire(), claraty::Frame::get_body(), claraty::ME_Body::get_name(), and claraty::Semaphore::release().

Referenced by get_body().

00494 {
00495   unsigned int index_pos = 0;
00496   // Wait until mech_model is available and acquire it to lock out other access
00497   _mech_model_sem.acquire();
00498   Tree<ME_Body>::Pre_Order_Iterator pre_iterate(_body_tree.begin_pre_order());
00499   Tree<ME_Body>::Pre_Order_Iterator end_iterate(_body_tree.end_pre_order());
00500   for (; pre_iterate < end_iterate; ++pre_iterate)
00501     {
00502       if (pre_iterate->get_name() == body_node.get_body()->get_name())
00503         break;
00504       ++index_pos;
00505     }
00506   _mech_model_sem.release(); // Release the semaphore lock
00507   return index_pos;
00508   
00509 }

Here is the call graph for this function:

unsigned int claraty::Mechanism_Model::_get_pre_order_body_index ( std::string body_name ) [private]

Get the index of a body

Parameters:

[in] body_name

Returns:: Index of the body.

Definition at line 520 of file mechanism_model.cc.

References _body_tree, _mech_model_sem, claraty::Semaphore::acquire(), and claraty::Semaphore::release().

00521 {
00522   unsigned int index_pos = 0;
00523   // Wait until mech_model is available and acquire it to lock out other access
00524   _mech_model_sem.acquire();
00525   Tree<ME_Body>::Pre_Order_Iterator pre_iterate(_body_tree.begin_pre_order());
00526   Tree<ME_Body>::Pre_Order_Iterator end_iterate(_body_tree.end_pre_order());
00527   for (; pre_iterate < end_iterate; ++pre_iterate)
00528     {
00529       if (pre_iterate->get_name() == body_name)
00530         break;
00531       ++index_pos;
00532     }
00533   _mech_model_sem.release(); // Release the semaphore lock
00534   return index_pos;
00535   
00536 }

Here is the call graph for this function:

Friends And Related Function Documentation

std::ostream& operator<<	(	std::ostream &	os,
		Mechanism_Model &	mm
	)			`[friend]`

Output to ostream

Parameters:

`[out]`	os	ostream to put output on.
`[in]`	mm	Mechanism_Model object to be output.

Returns:: ostream with output on it.

Definition at line 703 of file mechanism_model.cc.

00704 {
00705   // Put the file information in the string and write it to the file
00706   os << "<?xml version=\"1.0\" encoding=\"US-ASCII\"?>\n";
00707   os <<
00708     "<!DOCTYPE Mechanism_Model SYSTEM \"mobility_manipulation.dtd\">\n";
00709   os << "<!-- Automatically generated by Mechanism_Model -->\n";
00710 
00711   // Put in the mech_model name and version number
00712   os << "<Mechanism_Model name = \"";
00713   os << mm.get_name();
00714   os << "\" version = \"";
00715   os << mm.get_version();
00716   os << "\" >\n";
00717 
00718   XML_Out::increment_indentation_level();
00719   
00720   Tree<ME_Body>::Pre_Order_Iterator p_me_body_node
00721     = (mm.get_body_tree()).begin_pre_order();
00722   Tree<ME_Body>::Pre_Order_Iterator p_end_me_body_node
00723     = (mm.get_body_tree()).end_pre_order();
00724 
00725   // Skip over the base body
00726   ++p_me_body_node;
00727 
00728   for (; p_me_body_node < p_end_me_body_node; ++p_me_body_node)
00729     {
00730       os << *p_me_body_node;
00731     }
00732   
00733   XML_Out::decrement_indentation_level();
00734   
00735   // Write the end data to the file
00736   os << "</Mechanism_Model>\n\n<!-- EOF -->\n";
00737   
00738   return os;
00739 }