Categories

Implementing Custom Link Kinematics in Energid’s Actin SDK

Implementing Custom Link Kinematics in Energid’s Actin SDK

Energid’s Actin SDK is a C++ robotic control toolkit that can be used to model and simulate almost any type of manipulator. Users can define a robotic system, specify tasks and constraints (such as dynamic collision avoidance), and the Actin system will determine the necessary joint motions.

One of Actin’s advantages is its compatibility with many different types of robots. One feature I found particularly useful when using the SDK to control a non-traditional robotic device was the ability to define custom link kinematics. While Actin has standard link kinematics (such as prismatic or rotational) implemented, my task required an oval track joint where the robot would travel along an oval-shaped track. Fortunately, the Actin SDK provides a general base class, EcLinkKinematics, that can be used to implement custom link kinematics.

In this blog post, I will demonstrate how to use EcLinkKinematics by implementing a circle track joint class.

The Base Class, EcLinkKinematics

A custom link kinematic class is derived from Actin’s base class EcLinkKinematics, which can support any definable joint type with 1 degree of freedom.

#ifndef ecLinkKinematics_H_
#define ecLinkKinematics_H_
//------------------------------------------------------------------------------
// Copyright (c) 2003-2013 Energid Technologies. All rights reserved.
//
/// @file ecLinkKinematics.h
/// @class EcLinkKinematics
/// @brief Holds a description of a base class for link motion.
//
//------------------------------------------------------------------------------
#include <actinCore/actinCore_config.h>  // Required to be first header.
#include <xml/ecBaseExpTreeElement.h>
#include <foundCommon/ecCoordSysXForm.h>
#include <foundCommon/ecGeneralMotion.h>

// forward declarations
class EcArticulatedBodyDynamics;
class EcArticulatedBodyInertia;
class EcGeneralForce;
class EcJointActuator;

/// Holds a description of a base class for link motion. It supports general joint types 
/// with one degree of freedom.
class EC_ACTINCORE_MANIPULATOR_DECL EcLinkKinematics : public EcBaseExpressionTreeElement
{
public:
   /// unit type enumeration
   enum {LINEAR, ANGULAR, OTHER};

   /// default constructor - should not be used
   EcLinkKinematics ();

   /// destructor
   virtual ~EcLinkKinematics ();

   /// copy constructor
   EcLinkKinematics (const EcLinkKinematics& orig);

   /// assignment operator
   EcLinkKinematics& operator= (const EcLinkKinematics& orig);

   /// equality operator
   EcBoolean operator== (const EcLinkKinematics& orig) const;

   /// register components in this class
   virtual void registerComponents ();

   /// test for approximate equality
   virtual EcBoolean approxEq (const EcLinkKinematics& dh2, EcReal tol) const;

   /// kinematics functions

	/// get Primary Frame, which is the primary frame for the link represented in the
	/// link's DH frame

   /**
	\return The primary frame for this link. 
	This frame is used to specify physical extent, mass properties, and end effectors.
   */
   virtual const EcCoordinateSystemTransformation& primaryFrame () const;

   /// get Primary Frame Inverse
   virtual const EcCoordinateSystemTransformation& primaryFrameInverse () const;

   /// set Primary Frame
   virtual void setPrimaryFrame (const EcCoordinateSystemTransformation& value);

   /// calculate transformation
   /**
   \param[in] jointValue The joint value used to calculate the link transformation.
   \return The link transformation at jointValue.
   */
   virtual const EcCoordinateSystemTransformation& calculateTransform
      (
      EcReal jointValue
      ) const=0;

   /// transform an inboard transformation to the link's frame
   /**
   \param[in,out] xform The output transformation which is
	xform*calculateTransform(jointValue).
   \param[in] jointValue The joint value used to calculate the transformation.
   */
   virtual void transformBy
      (
      EcCoordinateSystemTransformation& xform,
      EcReal jointValue
      ) const=0;

   ///  get the general motion of the frame (locally represented).
   /**
	The default frame velocity calculation. 
	The velocity is the relative speed of the current frame to the parent frame
	represented in the current frame.  The default method will do a differentiation of
	the transformation, which is not as accurate if the close form solution of the frame
	velocity can be implemented in the child class.
   \param[in] jointValue The joint value.
   \param[in] jointVelocity The joint velocity.
   */
   virtual const EcGeneralMotion& calculateVelocity
      (
      EcReal jointValue,
      EcReal jointVelocity
      ) const;

   ///  get the general acceleration of the frame (locally represented).
   /**
	The default frame velocity calculation. The acceleration is the relative acceleration
	of the current frame to the parent frame represented in the current frame.  The
	default method will do a differentiation of the velocity, which is not as accurate if
	the close form solution of the frame acceleration can be implemented in the child
	class.
   \param[in] jointValue The joint value.
   \param[in] jointVelocity The joint velocity.
   \param[in] jointAcceleration The joint acceleration.
   */
   virtual const EcGeneralAcceleration& calculateAcceleration
      (
      EcReal jointValue,
      EcReal jointVelocity,
      EcReal jointAcceleration
      ) const;

   /// get the torque required to produce a unit acceleration of an
   /// articulated rigid body
   /**
   \param[in] jointValue The joint value.
   \param[in] inertia The articulated body inertia attached to the link.
   \return The torque required to produce a unit acceleration.
   */
   virtual EcReal unitAccelerationTorque
      (
      EcReal jointValue,
      const EcArticulatedBodyInertia& inertia
      ) const;

   /// get the torque required to exert the specified general force
   /**
   \param[in] jointValue The joint value.
   \param[in] exertedGeneralForce The desired general force.
   \return The torque required to exert the desired force.
   */
   virtual EcReal torqueRequiredToExert
      (
      EcReal jointValue,
      const EcGeneralForce& exertedGeneralForce
      ) const;

   /// compute an upper-bound distance between this DH
   /// and the parent
   virtual EcReal upperBoundDhFrameDistance
      (
      const EcJointActuator& jointActuator
      )const=0;

   /// add child arbd to parent's arbd
   virtual EcBoolean addChildArbdToParent
      (
      EcArticulatedBodyDynamics& parentArbd,
      const EcArticulatedBodyDynamics& childArbd,
      const EcReal jointValue,
      const EcReal jointTorque,
      const EcReal effectiveMotorInertia
      ) const;

   /// scale the kinematics by the specified distance-scale value
   /// (scaleFactor=1.0 does not change the object)
   virtual void scaleBy (EcReal scaleFactor)=0;

   /// transform this by a general coordinate system transformation.  This
   /// transformation is inserted before the link placement.  If the
   /// kinematics class has a precursor P, the P=XForm*P;
   virtual void transformBy (const EcCoordinateSystemTransformation& xform)=0;

   /// return the scale factor for the joint parameter (=1 for
   /// a distance-based parameter, 0 for non-distance-based)
   virtual EcReal jointParameterScalability ()const=0;

   /// change the inboard frame by an offset.
   /// This is to account for uncertainty in link measurements, both linear and angular.
   /// If offset is identity, then this does nothing.
   virtual void changeInboardFrameBy (const EcCoordinateSystemTransformation& offset)=0;

   /// return the unit type of the link kinematics (joint).
   virtual EcU32 unitType () const=0;

protected:

   /// the primary frame for the link represented in the link's DH frame
   EcCoordinateSystemTransformation m_PrimaryFrame;

   /// non-XML data below

   /// the inverse of the primary frame for the link represented in the link's DH frame
   /// this is stored for easy access
   EcCoordinateSystemTransformation m_PrimaryFrameInverse;

   /// a value for the transformation
   mutable EcCoordinateSystemTransformation m_FrameTransform;

   /// a value for the frame velocity
   mutable EcGeneralMotion m_FrameVelocity;

   /// a value for the frame Acceleration
   mutable EcGeneralAcceleration m_FrameAcceleration;

   // temporary variables for speed
   mutable EcGeneralMotion m_GMotion0;    /// First utility EcGeneralMotion
   mutable EcGeneralMotion m_GMotion1;    /// Second utility EcGeneralMotion
   mutable EcGeneralMotion m_GMotion2;    /// Third utility EcGeneralMotion
   /// First utility EcCoordinateSystemTransformation
   mutable EcCoordinateSystemTransformation m_XForm0;
      /// Second utility EcCoordinateSystemTransformation
   mutable EcCoordinateSystemTransformation m_XForm1;
};

#endif // ecLinkKinematics_H_

The Mathematical Model – The Transformation for the Circle Joint

At the heart of link kinematics is the equation that defines the transformation from the inner frame to the outer frame using a single joint angle parameter. In Actin, the inner frame is rigidly attached to the parent link, while the outer frame is rigidly attached to the link's physical extent. A frame is a coordinate system which defines a position and orientation, and a transformation defines the conversion between frames. In Actin, both concepts are represented through the EcCoordinateSystemTransformation class.

For our example, let the inner frame sit in the center of the circle, while the outer frame will travel along the circle’s perimeter while pointing its X-axis towards the center. The joint angle parameter will be the linear distance traveled along the circle’s perimeter. This transformation is defined in the transformBy method as shown below.

/////////////////////////////////////////////////////////////////////////
// Function:     transformBy
// Description:  transform an inboard transformation to the link's frame
//               The output is xform*FrameTrans(jointValue).
/////////////////////////////////////////////////////////////////////////
void CircleJoint::transformBy
(
	EcCoordinateSystemTransformation& xform,     // transformation
	EcReal jointValue                            	 // joint value
) const
{

	EcOrientation orientation;
	EcVector translation;

	// set to the precursor.
	xform *= precursor();

	translation.setX(m_Radius*cos(jointValue/m_Radius));
	translation.setY(m_Radius*sin(jointValue/m_Radius));
	translation.setZ(0);

	orientation.setFrom123Euler(0, 0, jointValue / m_Radius);

	// multiply by the transformation
	xform.outboardTransformBy(translation, orientation);
}

In this method definition, there is a reference to the precursor member variable, which is a transformation that occurs before the link kinematic's transformation. Without the precursor, this joint would always rotate about the inner frame’s Z-axis and move on the XY plane. However, with the precursor, this joint can rotate about a different axis. For example, to rotate about the Y-axis, the precursor transformation can be set to a 90° rotation about the X-axis.

The velocity and acceleration methods do not need to be defined explicitly, as Actin can automatically calculate them discretely, but they can be implemented for efficiency as shown below.

/////////////////////////////////////////////////////////////////////////
// Function:     calculateVelocity
// Description:  Returns a general velocity calculated from the joint 
//				 value and joint velocity
/////////////////////////////////////////////////////////////////////////
const EcGeneralMotion&
CircleJoint::calculateVelocity
(
	EcReal jointValue,
	EcReal jointVelocity
) const
{

	m_FrameVelocity.setLinearX(-jointVelocity * sin(jointValue / m_Radius));
	m_FrameVelocity.setLinearY(jointVelocity*cos(jointValue / m_Radius));
	m_FrameVelocity.setLinearZ(0);

	m_FrameVelocity.setAngular(EcVector(0, 0, jointVelocity / m_Radius));

	return m_FrameVelocity;
}

/////////////////////////////////////////////////////////////////////////
// Function:     calculateAcceleration
// Description:  Returns a general acceleration calculated from the joint 
//				 value, joint velocity, and joint acceleration
/////////////////////////////////////////////////////////////////////////
const EcGeneralAcceleration&
CircleJoint::calculateAcceleration
(
	EcReal jointValue,
	EcReal jointVelocity,
	EcReal jointAcceleration
) const
{

	m_FrameAcceleration.setLinearX(-jointAcceleration * sin(jointValue / m_Radius)
		- pow(jointVelocity, 2) / m_Radius * cos(jointValue / m_Radius));
	m_FrameAcceleration.setLinearY(jointAcceleration*cos(jointValue / m_Radius)
		- pow(jointVelocity, 2) / m_Radius * sin(jointValue / m_Radius));
	m_FrameAcceleration.setLinearZ(0);

	m_FrameAcceleration.setAngular(EcVector(0, 0, jointAcceleration / m_Radius));

	return m_FrameAcceleration;
}

XML Methods: Saving and Loading the Custom Link Kinematics

The EcLinkKinematics class derives from Actin’s EcXmlCompoundType, which has built-in XML functionality. This allows Actin to read and write our custom link kinematics class to an XML file, which Actin uses to save its models. Without this functionality, we would have to re-add the custom link kinematics to an Actin model file every time we load it.

For the member variables that must be saved to the XML file (in this example, the circle radius and precursor frame), XML tags need to be defined through the EcToken class (which holds a tag and a namespace), as shown in the header file below. These tokens are used by Actin’s XML parser to identify the data.

#ifndef CircleJointTokens_H_
#define CircleJointTokens_H_

#include <xml/ecToken.h>

namespace CircleJointNamespace
{
	const EcString NS = " CircleJointNamespace#";

	// Elements
	const EcToken CircleJointToken = NS + "CircleJoint";
	const EcToken CircleJointPrecursor = NS + "CircleJointPrecursor";
	const EcToken CircleJointRadiusToken = NS + "CircleJointRadius";

}

#endif // CircleJointTokens_H_

The member variable tokens are then registered using the registerComponents method.

/////////////////////////////////////////////////////////////////////////
// Functions:    registerComponents()
// Description:  registers all the components for this class that are
//               read and written to XML files.
/////////////////////////////////////////////////////////////////////////
void CircleJoint::registerComponents()
{
	// call parent
	EcLinkKinematics::registerComponents();

	registerComponent(CircleJointPrecursor, &m_Precursor);
	registerComponent(CircleJointRadiusToken, &m_Radius);
}

The class token can be defined using the ECXML_DEFINE_TOKENS macro.

ECXML_DEFINE_TOKENS(CircleJoint, CircleJointNamespace::CircleJointToken)

Actin can now read and write the class to an XML file, as shown below.

 
  
    
    
  
  2
  
    
    
  

The primaryFrame data is inherited from the base EcLinkKinematics class and used as a general reference for all links in Actin. Link parameters such as mass properties are defined relative to this frame.

Creating an XML Object Library – Loading the Custom Link Kinematics in ActinViewer

To avoid always including the source code, the custom link kinematics can be saved as a library. This library can also be used by ActinViewer (which is Energid’s Actin GUI Interface) to load and display the custom link kinematics. To save the library using Actin’s build system, we must define a libraryAttribute method.

EcXmlObject* CircleJoint::libraryAttribute()
{
	m_LibraryAttribute = "CircleJoint.ecp";
	return &m_LibraryAttribute;
}

The custom kinematics can be built into a library using Actin’s CMake macros.

ecProject(CircleJoint)

ecIncludeSolutions(
	stableFoundation
	foundation
	actinCore
	actin
)

ecExternIncludeDirs(Boost)

ecProjectLinkLibraries(
	actinCore - foundCommon
	stableFoundation - xmlReaderWriter
)

ecSourceFiles(
	CircleJoint.cpp
)

ecXmlObjectLibraryAndPlugin(
	CircleJoint
)

When built, this will generate a libecCircleJoint.so file.

Adding the Custom Link Kinematics into a Manipulator

Since our custom link kinematics is not part of the ActinViewer interface, we have to add it to a manipulator programmatically. This can be accomplished by defining a CircleJoint class and setting it as the link kinematics to the desired manipulator link.

//Create a manipulator link object
EcManipulatorLink link;

//Define our custom kinematics
CircleJoint cj;
cj.setRadius(2);
EcOrientation ori;
ori.setFrom123Euler(EcPi / 2, 0, 0);
EcCoordinateSystemTransformation xform;
xform.setOrientation(ori);
cj.setPrecursor(xform);

//Set as the link's kinematics
link.setLinkKinematics(cj);

When compiling this code, the Circle Joint library must be linked through the ecProjectLinkLibraries CMake macro.

ecProjectLinkLibraries(examples-CircleJoint)

I created the circle joint as a project within Actin’s examples solution (solutions are declared by the ecSolution() CMake macro), but the general form to this macro is ecProjectLinkLibraries(<solution name>-<project name>).

Complete Class

The above instructions summarize the basic steps to create a custom link kinematics in the Actin SDK. Included below is the full Circle Joint code.

Tokens Header File

// ----------------------------------------------------------------------------------
// Company: DMC
// Engineer: Sophia Dolan
//
// Create Date:    8/2017
// Module Name:    CircleJoint – circleJointTokens.h
// Project Name:   Example Custom Link Kinematics
// Description:    A link kinematics that represents a circular track joint
//
// ----------------------------------------------------------------------------------

#ifndef CircleJointTokens_H_
#define CircleJointTokens_H_

#include <xml/ecToken.h>

namespace CircleJointNamespace
{
	const EcString NS = "CircleJointNamespace#";

	// Elements
	const EcToken CircleJointToken = NS + "CircleJoint";
	const EcToken CircleJointPrecursor = NS + "CircleJointPrecursor";
	const EcToken CircleJointRadiusToken = NS + "CircleJointRadius";

}

#endif // CircleJointTokens_H_

Class Header File

 // ----------------------------------------------------------------------------------
// Company: DMC
// Engineer: Sophia Dolan
//
// Create Date:    8/2017
// Module Name:    CircleJoint – circleJointTokens.h
// Project Name:   Example Custom Link Kinematics
// Description:    A link kinematics that represents a circular track joint
//
// ----------------------------------------------------------------------------------


#ifndef ecCircleJoint_H_
#define ecCircleJoint_H_

#include <actinCore/actinCore_config.h>  // Required to be first header.
#include <foundCommon/ecCoordSysXForm.h>
#include <manipulator/ecLinkKinematics.h>
#include <manipulator/ecJointActuator.h>
#include <xml/ecXmlVectorType.h>

/// A type of joint that follows a geometrically defined rail
class EC_ACTINCORE_MANIPULATOR_DECL CircleJoint : public EcLinkKinematics
{
	EC_XMLOBJECT_PLUGIN_DECLARE(CircleJoint);
public:

	/// initialize XML components for reading and writing
	virtual EcBoolean xmlInit();

	/// register components with the parent
	virtual void registerComponents();

	/// static creator function
	static EcXmlObject* creator();

	/// get Precursor
	virtual const EcCoordinateSystemTransformation& precursor() const;

	/// set Precursor
	virtual void setPrecursor(const EcCoordinateSystemTransformation& value);

	/// get reference of radius of the oval curve (in meters)
	EcReal radius() const;

	/// set radius for the oval curve
	void setRadius(EcReal radius);

	/// calculate D-H transformation
	virtual const EcCoordinateSystemTransformation& calculateTransform(
    	EcReal jointValue
    ) const;

	/// transform an inboard transformation to the link's DH frame
	/// The output is xform*DH(jointValue).
	virtual void transformBy(
		EcCoordinateSystemTransformation& xform,
		EcReal jointValue
	) const;

	virtual const EcGeneralMotion& calculateVelocity(
		EcReal jointValue,
		EcReal jointVelocity
	) const;

	virtual const EcGeneralAcceleration& calculateAcceleration(
		EcReal jointValue,
		EcReal jointVelocity,
		EcReal jointAcceleration
	) const;

	using EcLinkKinematics::approxEq;
	/// test for approximate equality
	virtual EcBoolean approxEq(
		const CircleJoint& dh2,
		EcReal tol
	) const;

	/// computes the upper-bound DH frame distance for this joint
	virtual EcReal upperBoundDhFrameDistance(
		const EcJointActuator& jointActuator
	) const;

	/// scale the kinematics by the specified distance-scale value
	/// (scaleFactor=1.0 does not change the object)
	virtual void scaleBy(EcReal scaleFactor);

	/// transform this by a general coordinate system transformation.  This
	/// transformation is inserted before the link placement.  For
	/// precursor P, this gives P=XForm*P;
	virtual void transformBy(const EcCoordinateSystemTransformation& xform);

	/// return the scale factor for the joint parameter (=1 for
	/// a distance-based parameter, 0 for non-distance-based)
	virtual EcReal jointParameterScalability()const;

	/// change the inboard frame by an offset.
    /// this is to account for uncertainty in link measurements, both linear and angular.
    /// if offset is identity, then this does nothing.
	virtual void changeInboardFrameBy(const EcCoordinateSystemTransformation& offset);

	/// read this object from an XML stream
	virtual EcBoolean read(EcXmlReader& stream);

	/// write this object to an XML stream
	virtual EcBoolean write(EcXmlWriter& stream) const;

	/// get an zero offset
	static CircleJoint nullObject();

	virtual EcU32 unitType() const;

	// test class functionality
	EcBoolean selfTest() const;

private:

	/// the precursory transformation
	EcCoordinateSystemTransformation m_Precursor;

	/// the radius of the curved part of the oval
	EcXmlReal m_Radius;

	/// the joint value for the stored transformation;
	mutable EcReal m_JointValueForStoredTransform;
};

#endif // CircleJoint_H_

Class Implementation File

// ----------------------------------------------------------------------------------
// Company: DMC
// Engineer: Sophia Dolan
//
// Create Date:    8/2017
// Module Name:    CircleJoint – circleJointTokens.h
// Project Name:   Example Custom Link Kinematics
// Description:    A link kinematics that represents a circular track joint
//
// ----------------------------------------------------------------------------------
#include "circleJoint.h"

#include <manipulator/ecJointActuator.h>
#include <manipulator/ecManipulatorTokens.h>
#include <foundCore/ecMacros.h>
#include <foundCore/ecMath.h>
#include <math.h>
#include <xmlReaderWriter/ecXmlObjectReaderWriter.h>
#include "circleJointTokens.h"


// namespaces
using namespace CircleJointNamespace;

ECXML_DEFINE_TOKENS(CircleJoint, CircleJointNamespace::CircleJointToken)
EcXmlObject* CircleJoint::libraryAttribute()
{
	m_LibraryAttribute = "CircleJoint.ecp";
	return &m_LibraryAttribute;
}
/////////////////////////////////////////////////////////////////////////
// Functions:    Big four and other header functions.
// Description:  Common functions
/////////////////////////////////////////////////////////////////////////

// constructor
CircleJoint::CircleJoint() :
	EcLinkKinematics(),
	m_Precursor(EcCoordinateSystemTransformation::nullObject()),
	m_Radius(3.0),
	m_JointValueForStoredTransform(0.0)
{
}

// destructor
CircleJoint::~CircleJoint() {}

// copy constructor
CircleJoint::CircleJoint(const CircleJoint& orig) :
	EcLinkKinematics(orig),
	m_Precursor(orig.m_Precursor),
	m_Radius(orig.m_Radius),
	m_JointValueForStoredTransform(orig.m_JointValueForStoredTransform)
{}

// assignment operator
CircleJoint& CircleJoint::operator=(const CircleJoint& orig)
{
	// self assignment.
	if (this == &orig)
	{
		return *this;
	}

	// call parent
	EcLinkKinematics::operator=(orig);

	// copy data
	m_Precursor = orig.m_Precursor;
	m_Radius = orig.m_Radius;
	m_JointValueForStoredTransform = orig.m_JointValueForStoredTransform;

	return *this;
}

// equality operator
EcBoolean CircleJoint::operator==(const CircleJoint& orig) const
{
	// parent:
	EcBoolean retVal = EcLinkKinematics::operator==(orig);

	if (!(m_Precursor == orig.m_Precursor&&
		m_Radius == orig.m_Radius
		))
	{
		retVal = EcFalse;
	}

	return retVal;
}

/////////////////////////////////////////////////////////////////////////
// End of header functions
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
// Functions:    creator()
// Description:  A static creator function.  This is registered using
//               the class token.
// I/O:          Return XML object
/////////////////////////////////////////////////////////////////////////
EcXmlObject* CircleJoint::creator()
{
	EcXmlObject* retVal = new CircleJoint();

	return retVal;
}

/////////////////////////////////////////////////////////////////////////
// Functions:   xmlInit()
// Description: XML-initializes the class and all parent classes.
// I/O:         Return error status
/////////////////////////////////////////////////////////////////////////
EcBoolean CircleJoint::xmlInit()
{
	// parent
	return(EcLinkKinematics::xmlInit());

}

/////////////////////////////////////////////////////////////////////////
// Functions:    registerComponents()
// Description:  registers all the components for this class that are
//               read and written to XML files.
/////////////////////////////////////////////////////////////////////////
void CircleJoint::registerComponents()
{
	// call parent
	EcLinkKinematics::registerComponents();

	registerComponent(CircleJointPrecursor, &m_Precursor);
	registerComponent(CircleJointRadiusToken, &m_Radius);
}

/////////////////////////////////////////////////////////////////////////
// Functions:    get/set
// Description:  get/set methods
/////////////////////////////////////////////////////////////////////////

// get Precursor
const EcCoordinateSystemTransformation& CircleJoint::precursor() const
{
	return m_Precursor;
}

// set Precursor
void CircleJoint::setPrecursor(const EcCoordinateSystemTransformation& value)
{
	m_Precursor = value;
}

// get radius
EcReal CircleJoint::radius() const
{
	return m_Radius;
}

// set radius
void CircleJoint::setRadius(EcReal radius)
{
	m_Radius = radius;
}

/////////////////////////////////////////////////////////////////////////
// End of get/set.
/////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////
// Function:     calculateTransform
// Description:  calculate frame transformation from current to parent
// I/O:          see below
/////////////////////////////////////////////////////////////////////////
const EcCoordinateSystemTransformation& CircleJoint::calculateTransform
(
	EcReal jointValue  // joint value
) const
{

	// initialize, then transform
	m_JointValueForStoredTransform = jointValue;
	m_FrameTransform.setToIdentity();
	transformBy(m_FrameTransform, m_JointValueForStoredTransform);

	return m_FrameTransform;
}

/////////////////////////////////////////////////////////////////////////
// Function:     transformBy
// Description:  transform an inboard transformation to the link's frame
//               The output is xform*FrameTrans(jointValue).
// I/O:          see below
/////////////////////////////////////////////////////////////////////////
void CircleJoint::transformBy
(
	EcCoordinateSystemTransformation& xform,     // transformation
	EcReal jointValue                            // joint value
) const
{

	EcOrientation orientation;
	EcVector translation;

	// set to the precursor.
	xform *= precursor();

	translation.setX(m_Radius*cos(jointValue / m_Radius));
	translation.setY(m_Radius*sin(jointValue / m_Radius));
	translation.setZ(0);

	orientation.setFrom123Euler(0, 0, jointValue / m_Radius);

	// multiply by the transformation
	xform.outboardTransformBy(translation, orientation);
}

//------------------------------------------------------------------------------
const EcGeneralMotion&
CircleJoint::calculateVelocity
(
	EcReal jointValue,
	EcReal jointVelocity
) const
{

	m_FrameVelocity.setLinearX(-jointVelocity * sin(jointValue / m_Radius));
	m_FrameVelocity.setLinearY(jointVelocity*cos(jointValue / m_Radius));
	m_FrameVelocity.setLinearZ(0);

	m_FrameVelocity.setAngular(EcVector(0, 0, jointVelocity / m_Radius));

	return m_FrameVelocity;
}

//------------------------------------------------------------------------------
const EcGeneralAcceleration&
CircleJoint::calculateAcceleration
(
	EcReal jointValue,
	EcReal jointVelocity,
	EcReal jointAcceleration
) const
{

	m_FrameAcceleration.setLinearX(-jointAcceleration * sin(jointValue / m_Radius)
		- pow(jointVelocity, 2) / m_Radius * cos(jointValue / m_Radius));
	m_FrameAcceleration.setLinearY(jointAcceleration*cos(jointValue / m_Radius)
		- pow(jointVelocity, 2) / m_Radius * sin(jointValue / m_Radius));
	m_FrameAcceleration.setLinearZ(0);

	m_FrameAcceleration.setAngular(EcVector(0, 0, jointAcceleration / m_Radius));

	return m_FrameAcceleration;
}
/////////////////////////////////////////////////////////////////////////
// Function:     approxEq
// Description:  see if important parameters are within tolerance.
// I/O:          see below
/////////////////////////////////////////////////////////////////////////
EcBoolean CircleJoint::approxEq
(
	const CircleJoint& dh2,       // other link
	EcReal tol                    // tolerance
) const
{
	// parent AE
	EcBoolean parentAE = EcLinkKinematics::approxEq(dh2, tol);
	EcBoolean primaryAE = primaryFrame().approxEq(dh2.primaryFrame(), tol);
	EcBoolean radiusAE = fabs(m_Radius - dh2.radius()) < fabs(tol);

	return parentAE && primaryAE;
}

/////////////////////////////////////////////////////////////////////////
// Function:     scaleBy
// Description:  scale the kinematics by the specified distance-scale
//               value (scaleFactor=1.0 does not change the kinematics)
/////////////////////////////////////////////////////////////////////////
void CircleJoint::scaleBy
(
	EcReal scaleFactor
)
{
	// the precursor matrix
	m_Precursor.setTranslation(m_Precursor.translation()*scaleFactor);

	// Scale other parameters
	m_Radius = m_Radius * scaleFactor;

}

/////////////////////////////////////////////////////////////////////////
// Function:     transformBy
// Description:  transform by a general coordinate system transformation.
///              This transformation is inserted before the link placement.
///              For precursor P, this gives P=XForm*P;
// I/O:          Input: Xform
/////////////////////////////////////////////////////////////////////////
void CircleJoint::transformBy
(
	const EcCoordinateSystemTransformation& xform
)
{
	m_Precursor = xform * m_Precursor;
}

/////////////////////////////////////////////////////////////////////////
// Function:     jointParameterScalability
// Description:  return the scale factor for the joint parameter (=1 for
//               a distance-based parameter, 0 for non-distance-based)
// I/O:          Return: scale factor
/////////////////////////////////////////////////////////////////////////
EcReal CircleJoint::jointParameterScalability
(
) const
{
	// distance is the joint parameter
	return 1.0;
}
/////////////////////////////////////////////////////////////////////////
// Function:     read
// Description:  Reads class data from an XML stream.
// I/O:          Output: true for success.
/////////////////////////////////////////////////////////////////////////
EcBoolean CircleJoint::read
(
	EcXmlReader& stream
)
{
	EcBoolean retVal = EcXmlCompoundType::read(stream);

	// set the primary frame inverse
	m_PrimaryFrameInverse = m_PrimaryFrame.inverse();

	return retVal;
}

/////////////////////////////////////////////////////////////////////////
// Function:     write
// Description:  Writes class data to an XML stream.
// I/O:          Output: true for success.
/////////////////////////////////////////////////////////////////////////
EcBoolean CircleJoint::write
(
	EcXmlWriter& stream
) const
{
	EcBoolean retVal = EcXmlCompoundType::write(stream);

	return retVal;
}

/////////////////////////////////////////////////////////////////////////
// Function:     nullObject
// Description:  Create an empty object.
// I/O:          Return empty object
/////////////////////////////////////////////////////////////////////////
CircleJoint CircleJoint::nullObject()
{
	return CircleJoint();
}

EcU32 CircleJoint::unitType() const
{
	//only one type 
	return LINEAR;
}

/////////////////////////////////////////////////////////////////////////
// Function:     upperBoundDhFrameDistance
// Description:  computes the upper-bound DH frame distance for this joint
/////////////////////////////////////////////////////////////////////////
EcReal CircleJoint::upperBoundDhFrameDistance(const EcJointActuator& jointActuator)const
{
	EcReal retVal = 0;
	// get the max DB frame distance
	retVal = EcMath::maximum(
		calculateTransform
		(jointActuator.upperLimit()).translation().mag(),
		calculateTransform
		(jointActuator.lowerLimit()).translation().mag());

	return retVal;
}

/////////////////////////////////////////////////////////////////////////
// Function:     changeInboardFrameBy
// Description:  change the inboard frame by an offset.
/////////////////////////////////////////////////////////////////////////
void CircleJoint::changeInboardFrameBy
(
	const EcCoordinateSystemTransformation& offset
)
{
	m_Precursor *= offset;
}


//////////////////////////////////////////////////////////////////////////
// Test class functionality
/////////////////////////////////////////////////////////////////////////
EcBoolean CircleJoint::selfTest() const
{
	//Create test object
	CircleJoint rj1;
	rj1.setRadius(200);

	//Write to file
	EcString filename = EcString("CircleJointSelfTest.xml");
	EcXmlObjectReaderWriter::writeToFile(rj1, filename);

	//Read in from file
	CircleJoint rj2;
	EcXmlObjectReaderWriter::readFromFile(rj2, filename);

	//Compare the two objects
	if (!(rj1 == rj2))
	{
		EcPrint(Error) << "Circle Joint self test failed" << std::endl;
		return EcFalse;
	}

	return EcTrue;
}

Learn more about DMC's Robotic Automation and Integration services. Or, contact us to get started on a project today.

Comments

There are currently no comments, be the first to post one.

Post a comment

Name (required)

Email (required)

CAPTCHA image
Enter the code shown above:

Categories

Related Blog Posts