+ View the NASA Portal

Search JPL

Search CLARAty

- Algorithms- Infrastructure - Doxygen (public)- Doxygen (private)- Tutorials

- Repository- Nightly Build- Nightly Regression- Feature/Bug Tracking- Build Targets- Procedure- Conventions- Tools

me_joint.cc

Go to the documentation of this file.

// -*-c++-*-
//---------------------------< /-/ CLARAty /-/ >------------------------------
/**
 * @file me_joint.cc
 *
 * The ME_Joint object corresponds to the joint in a mechanism.
 * ME_Joint attaches a ME_Body to its parent.
 * 
 * <br>@b Designer(s):   Antonio Diaz-Calderon,
 *                       Won S. Kim,
 *                       Hari Das Nayar,
 *                       Issa Nesnas
 * <br>@b Author(s):     Hari Das Nayar
 * <br>@b Date:          March 24, 2004 
 *
 * <b>Software License:</b><br>
 * <code> http://claraty.jpl.nasa.gov/license/open_src/  or 
 *        file: license/open_src.txt </code> 
 *
 * &copy; 2006, Jet Propulsion Laboratory, California Institute of Technology<br>
 *
 * $Revision: 1.8 $
 */
//-----------------------------------------------------------------------------

#include "claraty/me_joint.h"
#include "claraty/mechanism_model.h"
#include "claraty/me_body.h"
#include "claraty/xml_out.h"
#include "claraty/string_util.h"

using namespace std;

namespace claraty {

//----------------------------------------------------------------------------
/**
 * Evaluates the constraint expression and assigns the parts of the
 * constraint equation to their respective elements. The constraint
 * expression should have the form:
 *   [-]slope * dependent_body_name[(joint_value_index)] [+/- offset]
 * where
 *     slope is the required multiplication factor for the dependence.
 *         slope may have an optional negative sign before it.
 *     dependent_body_name is the required name of the body that this
 *         joint's constraint depends on
 *     [(joint_value_index)] is optional and designates the index of
 *         the joint value to depend on for multi-dof joints. The default
 *         if joint_value_index is not specified is 0.
 *     [+/- offset] is the optional positive or negative offset value
 *         in the constraint equation.
 *
 * @return true if successfully parsed the string expression, false if
 *         failed.
 *
 * @ingroup mech_model_pkg
 */
bool Joint_Constraint::evaluate_constraint_expression()
{
  double slope_sign = 1.0;
  _dependent_body_joint_value_index = 0;

  // Make a copy of the string expression
  std::string string_expression(s_expr_attribute);

  // Remove white space from the expression and make a copy
  for (unsigned int i=0; i<string_expression.length(); ++i)
    {
      if ((string_expression[i] == ' ')||(string_expression[i] == '\t')
          ||(string_expression[i] == '\n')||(string_expression[i] == '\r'))
        {
          string_expression.erase(i, 1);
          --i;
        }
    }

  if (string_expression[0] == '-')
    {
      slope_sign = -1.0;
      string_expression.erase(0,1);
    }

  std::string string_expression_copy = string_expression;

  // if the expression is empty, return false
  if (string_expression.length() == 0)
    {
    return false;
    }
  // If there is no * char in the string, return false
  if (string_expression.find("*", 0) == std::string::npos)
    {

    return false;
    }
  string delims("*+-)( \t\n\r");
  string tokens("");
  vector<string> out_list = string_get_tokens(string_expression,
                                              delims,
                                              tokens);

  // Check that the correst number of elements were found in the expression
  if ((out_list.size() > 4) || (out_list.size() < 2))
    {
      return false;
    }

  // Check that the multiplication factor was in the right place
  if (string_expression_copy[string_expression_copy.find(out_list[1])-1] != '*')
    {
      return false;
    }

  if (out_list.size() == 3)
    {
      if (string_expression_copy[string_expression_copy.find(out_list[2])-1]
          == '(')
        {
          string_eat_long(out_list[2], _dependent_body_joint_value_index);
        }
      else if (string_expression_copy[
                     string_expression_copy.find(out_list[2])-1] == '+')
        {
          string_eat_double(out_list[2], _offset);
        }
      else if (string_expression_copy[
                    string_expression_copy.find(out_list[2])-1] == '-')
        {
          string_eat_double(out_list[2], _offset);
          _offset = - _offset;
        }
      else
        {
          return false;
        }
    }

  if (out_list.size() == 4)
    {
      if (string_expression_copy[string_expression_copy.find(out_list[2])-1]
          == '(')
        {
          string_eat_long(out_list[2], _dependent_body_joint_value_index);
        }
      else
        {
          return false;
        }

      if (string_expression_copy[string_expression_copy.find(out_list[3])-1]
          == '+')
        {
          string_eat_double(out_list[3], _offset);
        }
      else if (string_expression_copy[
                     string_expression_copy.find(out_list[3])-1] == '-')
        {
          string_eat_double(out_list[3], _offset);
          _offset = - _offset;
        }
      else
        {
          return false;
        }
    }

  string_eat_double(out_list[0], _slope);
  _slope *= slope_sign;

  _dependent_body_name = out_list[1];

  return true;

}

//----------------------------------------------------------------------------
/**
 * Default Constructor for a ME_Joint object.
 *
 * @ingroup mech_model_pkg
 */
ME_Joint::ME_Joint()
  : _limited(false),
    _number_of_dofs(1),
    _parent_frame(0),
    _constrained(false),
    _stiffness(0),
    _constraint(0)
{
  _values.resize(_number_of_dofs);
  _joint_axis.resize(3);
  _joint_axis(0) = 0.0;
  _joint_axis(1) = 0.0;
  _joint_axis(2) = 1.0;
}

//----------------------------------------------------------------------------
/**
 * Copy Constructor for a ME_Joint object.
 *
 * @param[in] me_joint The joint to be copied.
 *
 * @ingroup mech_model_pkg
 */
ME_Joint::ME_Joint(ME_Joint & me_joint)
: _index(me_joint._index),
  _limited(me_joint._limited),
  _offset(me_joint._offset),
  _home(me_joint._home),
  _number_of_dofs(me_joint._number_of_dofs),
  _parent_frame(me_joint._parent_frame),        
  _constrained(me_joint._constrained),
  _name(me_joint._name),
  _limits(me_joint._limits)

{
  if (me_joint._parent_frame)
    _parent_frame = new Frame(*me_joint._parent_frame);

  _limits = me_joint._limits;
  if (me_joint._stiffness)
    {
      _stiffness = new Joint_Stiffness(*me_joint._stiffness);
    }
  if (me_joint._constraint)
    {
      _constraint = new Joint_Constraint(*me_joint._constraint);
    }

  set_type(me_joint._type);
  _joint_axis.resize(3);
  _joint_axis = me_joint._joint_axis;
  _values.resize(_number_of_dofs);
}


//----------------------------------------------------------------------------
/**
 * Get the number of dofs
 *
 * @return number of dofs
 *
 * @ingroup mech_model_pkg
 */
int ME_Joint::get_number_dofs()
{
  return _number_of_dofs;
}

//----------------------------------------------------------------------------
/**
 * Return the joint name
 *
 * @return joint name string
 *
 * @ingroup mech_model_pkg
 */
std::string ME_Joint::get_name()
{
  return _name;
}
//----------------------------------------------------------------------------
/**
 * Set name.
 *
 * @param[in] name
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::set_name(std::string name)
 {
  _name = name;
}

//----------------------------------------------------------------------------
/**
 * 
 * Get type.
 * @return Type.
 *
 * @ingroup mech_model_pkg
 */
std::string ME_Joint::get_type()
{
  return _unresolve_type(_type);
}

//----------------------------------------------------------------------------
/**
 * 
 * Get vector values.
 *
 * @return Vector.
 *
 * @ingroup mech_model_pkg
 */
vector<double> & ME_Joint::get_values_vector()
{
  if (_constrained)
    {
      _values[0] = get_value(0);
    }
  return _values;
}


//----------------------------------------------------------------------------
/**
 * Set joint axes.
 *
 * @param[in] joint_axis  Vector of joint axis.
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::set_joint_axes(Vector<double> joint_axis)
{
  _joint_axis(0) = joint_axis(0);
  _joint_axis(1) = joint_axis(1);
  _joint_axis(2) = joint_axis(2);
}

//----------------------------------------------------------------------------
/**
 * Set joint axes.
 *
 * @param[in] joint_axis  Vector of joint axis.
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::get_joint_axes(Vector<double> & joint_axis)
{
  joint_axis(0) = _joint_axis(0);
  joint_axis(1) = _joint_axis(1);
  joint_axis(2) = _joint_axis(2);
}

//----------------------------------------------------------------------------
/**
 * Get joint axis.
 *
 * @return Vector.
 *
 * @ingroup mech_model_pkg
 */
Vector<double>& ME_Joint::get_joint_axis()
{
  return _joint_axis;
}
//----------------------------------------------------------------------------
/**
 * Rotate the joint axis by the specified rotation.
 *
 * @param[in] rotation  Quaternion specifying rotation
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::rotate_joint_axes(const Quaternion<double> & rotation)
{
  Quaternion<double> rotation_conjugate(rotation);
  rotation_conjugate.conjugate();

  set_joint_axes(rotation_conjugate * Point<double>(_joint_axis));
}
//----------------------------------------------------------------------------
/**
 * Rotate the joint axis by the specified rotation.
 *
 * @param[in] rotation Quaternion specifying rotation
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::rotate_joint_axes(const RMatrix<double> & rotation)
{
  Quaternion<double> q_rotation(rotation);
  rotate_joint_axes(q_rotation);
}
//----------------------------------------------------------------------------
/**
 * Set joint type.
 *
 * @param[in] type
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::set_type(short type)
{
  _type = type;
  switch(_type)
    {
    case PIN:
      _number_of_dofs = 1;
      break;
    case SLIDER:
      _number_of_dofs = 1;
      break;
    case FULL6DOF:
      _number_of_dofs = 6;
      break;
    case STATIC:
      _number_of_dofs = 0;
      break;
    default:
      break;
    }
  _values.resize(_number_of_dofs);
}
//----------------------------------------------------------------------------
/**
 * Set joint type.
 *
 * @param[in] type
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::set_type(std::string type)
{
  _type = _resolve_type(type);
  switch(_type)
    {
    case PIN:
      _number_of_dofs = 1;
      break;
    case SLIDER:
      _number_of_dofs = 1;
      break;
    case FULL6DOF:
      _number_of_dofs = 6;
      break;
    case STATIC:
      _number_of_dofs = 0;
      break;
    default:
      break;
    }
  _values.resize(_number_of_dofs);
}

//----------------------------------------------------------------------------
/**
 * Interogate if joing is actuated.
 *
 * @return Ture if actuated, false otherwise.
 *
 * @ingroup mech_model_pkg
 */
bool ME_Joint::is_actuated()
{
  return _actuated;
}

//----------------------------------------------------------------------------
/**
 * Set joint actuated value.
 *
 * @param[in] actuated
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::set_actuated(bool actuated)
{
  _actuated = actuated;
}

//----------------------------------------------------------------------------
/**
 * Get joint offset
 *
 * @return Joint offset.
 *
 * @ingroup mech_model_pkg
 */
double & ME_Joint::get_offset()
{
  return _offset;
}

//----------------------------------------------------------------------------
/**
 * Compute the joint value based on the constraint parameters.
 *
 * @param[in] dof_index
 *
 * @return Joint value.
 *
 * @ingroup mech_model_pkg
 */
double ME_Joint::get_value(int dof_index)
{
  if (_number_of_dofs == 0)
    return 0.0;
  if (!_constrained)
    return _values[dof_index];
  else  // compute the joint joint value based on the constraint parameters
    {
      return _constraint->get_slope() *
        _me_body->get_tree_body(
             _constraint->get_dependent_body_name()).get_joint().get_value(
                                      _constraint->get_dependent_joint_index())
        + _constraint->get_offset();
    }
}
//----------------------------------------------------------------------------
/**
 * Set joint value.
 *
 * @param[in] value
 * @param[in] dof_index
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::set_value(double value, int dof_index)
{
  if (_number_of_dofs != 0)
  _values[dof_index] = value;
}
//----------------------------------------------------------------------------
/**
 *  Get joint transform.
 *
 * @return Transform.
 *
 * @ingroup mech_model_pkg
 */
Transform ME_Joint::get_transform()
{
  Transform result;
  switch(_type)
    {
    case PIN:
      result = result.rotate_z(_values[0]);
      break;
    case SLIDER:
      result = result.translate_z(_values[0]);
      break;
    case FULL6DOF:
      result = Transform(Point<double>(_values[0], _values[1],_values[2]),
                      RMatrix<double>(RPY,_values[3], _values[4], _values[5]));
      break;
    case STATIC:
      result = Transform::identity;
    }
  return result;
}

//----------------------------------------------------------------------------
/**
 * Set joint offset
 *
 * @param[in] offset
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::set_offset(double offset)
{
  _offset = offset;
}

//----------------------------------------------------------------------------
/**
 * Get joint home.
 *
 * @return Home.
 *
 * @ingroup mech_model_pkg
 */
double & ME_Joint::get_home()
{
  return _home;
}

//----------------------------------------------------------------------------
/**
 * Set joint home.
 *
 * @param[in] home
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::set_home(double home)
{
  _home = home;
}

//----------------------------------------------------------------------------
/**
 * Get joint type as a string, revolute, prismatic, full6dof, or fixed_mount.
 *
 * @param[in] joint_type
 *
 * @return String representing the joint type.
 *
 * @ingroup mech_model_pkg
 */
std::string ME_Joint::_unresolve_type(int joint_type)
{
  if (joint_type == PIN)
    return std::string("revolute");
  
  if (joint_type == SLIDER)
    return std::string("prismatic");
  
  if (joint_type == FULL6DOF)
    return std::string("full6dof");
  
  if (joint_type == STATIC)
    return std::string("fixed_mount");
  
  return std::string("");
}
//----------------------------------------------------------------------------
/**
 * Take the string for a joint type and returns the integer which represents
 * this joint type.
 *
 * @param[in] joint_type
 * @return The integer representing the joint type.
 *
 * @ingroup mech_model_pkg
 */
int ME_Joint::_resolve_type(std::string joint_type)
{
  if (strcmp(joint_type.c_str(), "revolute") == 0)
    return PIN;
  
  if (strcmp(joint_type.c_str(), "prismatic") == 0)
    return SLIDER;
  
  if (strcmp(joint_type.c_str(), "full6dof") == 0)
    return FULL6DOF;
  
  if (strcmp(joint_type.c_str(), "fixed_mount") == 0)
    return STATIC;
  
  return UNDEFINED;
}

//----------------------------------------------------------------------------
/**
 * Get joint limits.
 *
 * @param[out] joint_min
 * @param[out] joint_max
 * @param[out] joint_vmax
 * @param[out] joint_tmin
 * @param[out] joint_tmax
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::get_limits(double & joint_min, double & joint_max,
                                double & joint_vmax,
                                double & joint_tmin, double & joint_tmax)
{
  _limits.get_parameters(joint_min, joint_max, joint_vmax,
                                joint_tmin, joint_tmax);
}

//----------------------------------------------------------------------------
/**
 * Set joint limits.
 *
 * @param[in] joint_min
 * @param[in] joint_max
 * @param[in] joint_vmax
 * @param[in] joint_tmin
 * @param[in] joint_tmax
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::set_limits(double joint_min, double joint_max,
                                double joint_vmax,
                                double joint_tmin, double joint_tmax)
{  
  _limits.set_parameters(joint_min, joint_max, joint_vmax,
                                joint_tmin, joint_tmax);
  _limited = true;
}

//----------------------------------------------------------------------------
/**
 * Get joint stiffness.
 *
 * @param[out] kx
 * @param[out] ky
 * @param[out] kz
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::get_stiffness(double & kx, double & ky, double & kz)
{
  if(_stiffness)
    _stiffness->get_parameters(kx, ky, kz);
  else
    kx = 0.0; ky = 0.0; kz = 0.0;
}

//----------------------------------------------------------------------------
/**
 * Set joint stiffness.
 *
 * @param[in] kx
 * @param[in] ky
 * @param[in] kz
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::set_stiffness(double kx, double ky, double kz)
{
  if (!_stiffness)
    _stiffness = new Joint_Stiffness();
  _stiffness->set_parameters(kx, ky, kz);
}

//----------------------------------------------------------------------------
/**
 * Get joint constraint expression.
 *
 * @return String of the expression for the constraints.
 *
 * @ingroup mech_model_pkg
 */
std::string ME_Joint::get_constraint_expression()
{
  if (_constraint)
    return _constraint->get_parameters();
  else
    return ("");
}
//----------------------------------------------------------------------------
/**
 * Set joint contstraint expression.
 *
 * @param[in]  expr The joint constraint expression.
 *
 * @ingroup mech_model_pkg
 */
void ME_Joint::set_constraint_expression(std::string expr)
{
  if (!_constraint)
    _constraint = new Joint_Constraint;
  _constraint->set_parameters(expr);
  if ((expr.length() > 0) &&(_constraint->evaluate_constraint_expression()))
    _constrained = true;
  else
    {
      delete _constraint;
      _constraint = NULL;
      _constrained = false;
    }

}
//----------------------------------------------------------------------------
/**
 * Output to ostream
 *
 * @param[out] os  ostream to put output on.
 * @param[in]  me_joint  ME_Joint object to be output.
 * @return ostream with output on it.
 *
 * @ingroup mechanism_model_io_pkg
 */
std::ostream& operator<< (std::ostream& os, ME_Joint & me_joint)
{
  std::string spacing = "";
  std::string child_spacing = "";
  spacing.append(XML_Out::get_indentation_space(), ' ');
  child_spacing.append(XML_Out::get_indentation_space()
                       + XML_Out::get_tab_space(), ' ');  
  os << spacing;
  os << "<ME_Joint ";
  os << "name = \""     << me_joint.get_name() << "\" ";
  
  os << "type = \""     << me_joint.get_type() << "\" ";
  
  os << "actuated = \"" << (me_joint.is_actuated() ? "true" : "false")
     << "\" " ;
  
  os << "offset = \""   << me_joint.get_offset() << "\" ";

  os << "home = \""     << me_joint.get_home() << "\" ";
  
  //  Vector<double> joint_axis(3);
  //  me_joint.get_joint_axes(joint_axis);
  //  os << "x_axis = \""     << joint_axis(0) << "\" ";
  //  os << "y_axis = \""     << joint_axis(1) << "\" ";
  //  os << "z_axis = \""     << joint_axis(2) << "\" ";

  os << ">" << endl;
  

  double jmin, jmax, vmax, tmin, tmax;
  me_joint.get_limits(jmin, jmax, vmax, tmin, tmax);
  os << child_spacing;
  os << "<Joint_Limits ";
  os << "min = \""         <<  jmin << "\" ";
  os << "max = \""         <<  jmax << "\" ";
  os << "vmax = \""        <<  vmax << "\" ";
  os << "torque_min = \""  <<  tmin << "\" ";
  os << "torque_max = \""  <<  tmax << "\" ";
  os << "/>" << endl;
  
  double kx, ky, kz;
  me_joint.get_stiffness(kx, ky, kz);
  os << child_spacing;
  os << "<Joint_Stiffness ";
  os << "kx = \""  << kx << "\" ";
  os << "ky = \""  << ky << "\" ";
  os << "kz = \""  << kz << "\" ";
  os << "/>" << endl;
  
  os << child_spacing;
  os << "<Joint_Constraint ";
  os << "expr = \"" << me_joint.get_constraint_expression() << "\" ";
  os << "/>" << endl;
  
  os << spacing;
  os << "</ME_Joint> \n";

  return os;
}
//----------------------------------------------------------------------------
//

} // namespace claraty

Last modified: June 05 2007 10:19:53

JPL PRIVACY STATEMENT GLOSSARY SITEMAP FEEDBACK CONTACT US

Site Manager: Issa A.D. Nesnas
Site Maintainer: Kelly Breed
Document Clearance #: CL 07-0635