Actin SDK from Energid, now Universal Robotics, 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.
C++
#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.
C++
/////////////////////////////////////////////////////////////////////////
// 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.
C++
/////////////////////////////////////////////////////////////////////////
// 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.
C++
#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.
C++
/////////////////////////////////////////////////////////////////////////
// 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.
C++
ECXML_DEFINE_TOKENS(CircleJoint, CircleJointNamespace::CircleJointToken)Actin can now read and write the class to an XML file, as shown below.
XML
<mynamespace:circlejoint library="CircleJoint.ecp" xmlns:mynamespace="MyNameSpace">
<mynamespace:circlejointprecursor>
<cr:orientation q0=".7071067811865476" q1=".7071067811865475" q2="-0" q3="0" xmlns:cr="http://www.energid.com/namespace/cr">
<translation x="0" y="0" z="0">
</translation></cr:orientation></mynamespace:circlejointprecursor>
<mynamespace:circlejointradius>2</mynamespace:circlejointradius>
<primaryframe>
<cr:orientation q0="1" q1="0" q2="0" q3="0" xmlns:cr="http://www.energid.com/namespace/cr">
<translation x="0" y="0" z="0">
</translation></cr:orientation></primaryframe>
</mynamespace:circlejoint>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.
C++
EcXmlObject* CircleJoint::libraryAttribute()
{
m_LibraryAttribute = "CircleJoint.ecp";
return &m_LibraryAttribute;
}The custom kinematics can be built into a library using Actin’s CMake macros.
DSL
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.
C++
//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.
DSL
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
C++
// ----------------------------------------------------------------------------------
// 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
C++
// ----------------------------------------------------------------------------------
// Company: DMC
// Engineer: Sophia Dolan
//
// Create Date : 08/2017
// Module Name : CircleJoint
// File Name : ecCircleJoint.h
// Project : Example Custom Link Kinematics
// Description :
// A custom link kinematics implementation that represents a
// circular track joint.
// ----------------------------------------------------------------------------------
#ifndef EC_CIRCLEJOINT_H_
#define EC_CIRCLEJOINT_H_
#include <actinCore/actinCore_config.h> // Must be included first
#include <foundCommon/ecCoordSysXForm.h>
#include <manipulator/ecJointActuator.h>
#include <manipulator/ecLinkKinematics.h>
#include <xml/ecXmlVectorType.h>
/// A joint that follows a geometrically defined circular rail.
class EC_ACTINCORE_MANIPULATOR_DECL CircleJoint : public EcLinkKinematics
{
EC_XMLOBJECT_PLUGIN_DECLARE(CircleJoint);
public:
/// Initialize XML components for serialization.
virtual EcBoolean xmlInit();
/// Register XML components with the parent object.
virtual void registerComponents();
/// Factory method.
static EcXmlObject* creator();
/// Get precursor transformation.
virtual const EcCoordinateSystemTransformation& precursor() const;
/// Set precursor transformation.
virtual void setPrecursor(
const EcCoordinateSystemTransformation& value
);
/// Get circular track radius (meters).
EcReal radius() const;
/// Set circular track radius.
void setRadius(EcReal radius);
/// Calculate the D-H transformation for the supplied joint value.
virtual const EcCoordinateSystemTransformation& calculateTransform(
EcReal jointValue
) const;
/// Transform an inboard transformation to the link's D-H frame.
/// Result:
///
/// xform = xform * DH(jointValue)
///
virtual void transformBy(
EcCoordinateSystemTransformation& xform,
EcReal jointValue
) const;
/// Calculate joint velocity.
virtual const EcGeneralMotion& calculateVelocity(
EcReal jointValue,
EcReal jointVelocity
) const;
/// Calculate joint acceleration.
virtual const EcGeneralAcceleration& calculateAcceleration(
EcReal jointValue,
EcReal jointVelocity,
EcReal jointAcceleration
) const;
using EcLinkKinematics::approxEq;
/// Test for approximate equality.
virtual EcBoolean approxEq(
const CircleJoint& other,
EcReal tolerance
) const;
/// Compute the upper-bound D-H frame distance for this joint.
virtual EcReal upperBoundDhFrameDistance(
const EcJointActuator& jointActuator
) const;
/// Scale the kinematics by the specified factor.
///
/// scaleFactor = 1.0 leaves the object unchanged.
virtual void scaleBy(EcReal scaleFactor);
/// Apply a coordinate transformation before link placement.
///
/// For precursor P:
///
/// P = xform * P
///
virtual void transformBy(
const EcCoordinateSystemTransformation& xform
);
/// Return the parameter scalability factor.
///
/// Returns:
/// 1.0 for distance-based parameters
/// 0.0 for non-distance-based parameters
///
virtual EcReal jointParameterScalability() const;
/// Modify the inboard frame by an offset transformation.
///
/// Used to account for uncertainty in link measurements,
/// both translational and rotational.
virtual void changeInboardFrameBy(
const EcCoordinateSystemTransformation& offset
);
/// Read object from XML.
virtual EcBoolean read(EcXmlReader& stream);
/// Write object to XML.
virtual EcBoolean write(EcXmlWriter& stream) const;
/// Return a default/empty object.
static CircleJoint nullObject();
/// Return unit type identifier.
virtual EcU32 unitType() const;
/// Run internal validation tests.
EcBoolean selfTest() const;
private:
/// Precursor transformation.
EcCoordinateSystemTransformation m_Precursor;
/// Radius of the circular track.
EcXmlReal m_Radius;
/// Cached joint value associated with the stored transform.
mutable EcReal m_JointValueForStoredTransform;
};
#endif // EC_CIRCLEJOINT_H_Class Implementation File
C++
// ----------------------------------------------------------------------------------
// Company: DMC
// Engineer: Sophia Dolan
//
// Create Date : 08/2017
// Module : CircleJoint
// File : circleJoint.cpp
// Project : Example Custom Link Kinematics
// Description :
// Custom link kinematics representing a circular track joint.
// ----------------------------------------------------------------------------------
#include "circleJoint.h"
#include "circleJointTokens.h"
#include <foundCore/ecMacros.h>
#include <foundCore/ecMath.h>
#include <manipulator/ecJointActuator.h>
#include <manipulator/ecManipulatorTokens.h>
#include <math.h>
#include <xmlReaderWriter/ecXmlObjectReaderWriter.h>
using namespace CircleJointNamespace;
ECXML_DEFINE_TOKENS(
CircleJoint,
CircleJointNamespace::CircleJointToken
)
EcXmlObject* CircleJoint::libraryAttribute()
{
m_LibraryAttribute = "CircleJoint.ecp";
return &m_LibraryAttribute;
}
//------------------------------------------------------------------------------
// Constructors / Destructor
//------------------------------------------------------------------------------
CircleJoint::CircleJoint() :
EcLinkKinematics(),
m_Precursor(EcCoordinateSystemTransformation::nullObject()),
m_Radius(3.0),
m_JointValueForStoredTransform(0.0)
{
}
CircleJoint::~CircleJoint()
{
}
CircleJoint::CircleJoint(const CircleJoint& other) :
EcLinkKinematics(other),
m_Precursor(other.m_Precursor),
m_Radius(other.m_Radius),
m_JointValueForStoredTransform(other.m_JointValueForStoredTransform)
{
}
CircleJoint& CircleJoint::operator=(const CircleJoint& other)
{
if (this == &other)
{
return *this;
}
EcLinkKinematics::operator=(other);
m_Precursor = other.m_Precursor;
m_Radius = other.m_Radius;
m_JointValueForStoredTransform =
other.m_JointValueForStoredTransform;
return *this;
}
EcBoolean CircleJoint::operator==(const CircleJoint& other) const
{
return
EcLinkKinematics::operator==(other) &&
m_Precursor == other.m_Precursor &&
m_Radius == other.m_Radius;
}
//------------------------------------------------------------------------------
// XML Support
//------------------------------------------------------------------------------
EcXmlObject* CircleJoint::creator()
{
return new CircleJoint();
}
EcBoolean CircleJoint::xmlInit()
{
return EcLinkKinematics::xmlInit();
}
void CircleJoint::registerComponents()
{
EcLinkKinematics::registerComponents();
registerComponent(
CircleJointPrecursor,
&m_Precursor
);
registerComponent(
CircleJointRadiusToken,
&m_Radius
);
}
//------------------------------------------------------------------------------
// Accessors
//------------------------------------------------------------------------------
const EcCoordinateSystemTransformation&
CircleJoint::precursor() const
{
return m_Precursor;
}
void CircleJoint::setPrecursor(
const EcCoordinateSystemTransformation& value
)
{
m_Precursor = value;
}
EcReal CircleJoint::radius() const
{
return m_Radius;
}
void CircleJoint::setRadius(EcReal radius)
{
m_Radius = radius;
}
//------------------------------------------------------------------------------
// Kinematics
//------------------------------------------------------------------------------
const EcCoordinateSystemTransformation&
CircleJoint::calculateTransform(
EcReal jointValue
) const
{
m_JointValueForStoredTransform = jointValue;
m_FrameTransform.setToIdentity();
transformBy(
m_FrameTransform,
m_JointValueForStoredTransform
);
return m_FrameTransform;
}
void CircleJoint::transformBy(
EcCoordinateSystemTransformation& xform,
EcReal jointValue
) const
{
EcOrientation orientation;
EcVector translation;
xform *= precursor();
const EcReal angle = jointValue / m_Radius;
translation.setX(
m_Radius * cos(angle)
);
translation.setY(
m_Radius * sin(angle)
);
translation.setZ(0.0);
orientation.setFrom123Euler(
0.0,
0.0,
angle
);
xform.outboardTransformBy(
translation,
orientation
);
}
//------------------------------------------------------------------------------
// Velocity / Acceleration
//------------------------------------------------------------------------------
const EcGeneralMotion&
CircleJoint::calculateVelocity(
EcReal jointValue,
EcReal jointVelocity
) const
{
const EcReal angle = jointValue / m_Radius;
m_FrameVelocity.setLinearX(
-jointVelocity * sin(angle)
);
m_FrameVelocity.setLinearY(
jointVelocity * cos(angle)
);
m_FrameVelocity.setLinearZ(0.0);
m_FrameVelocity.setAngular(
EcVector(
0.0,
0.0,
jointVelocity / m_Radius
)
);
return m_FrameVelocity;
}
const EcGeneralAcceleration&
CircleJoint::calculateAcceleration(
EcReal jointValue,
EcReal jointVelocity,
EcReal jointAcceleration
) const
{
const EcReal angle = jointValue / m_Radius;
const EcReal velocitySquared =
jointVelocity * jointVelocity;
m_FrameAcceleration.setLinearX(
-jointAcceleration * sin(angle)
- velocitySquared / m_Radius * cos(angle)
);
m_FrameAcceleration.setLinearY(
jointAcceleration * cos(angle)
- velocitySquared / m_Radius * sin(angle)
);
m_FrameAcceleration.setLinearZ(0.0);
m_FrameAcceleration.setAngular(
EcVector(
0.0,
0.0,
jointAcceleration / m_Radius
)
);
return m_FrameAcceleration;
}
//------------------------------------------------------------------------------
// Utility Functions
//------------------------------------------------------------------------------
EcBoolean CircleJoint::approxEq(
const CircleJoint& other,
EcReal tol
) const
{
const EcBoolean parentEqual =
EcLinkKinematics::approxEq(
other,
tol
);
const EcBoolean frameEqual =
primaryFrame().approxEq(
other.primaryFrame(),
tol
);
const EcBoolean radiusEqual =
fabs(m_Radius - other.radius()) <
fabs(tol);
return
parentEqual &&
frameEqual &&
radiusEqual;
}
void CircleJoint::scaleBy(
EcReal scaleFactor
)
{
m_Precursor.setTranslation(
m_Precursor.translation() *
scaleFactor
);
m_Radius *= scaleFactor;
}
void CircleJoint::transformBy(
const EcCoordinateSystemTransformation& xform
)
{
m_Precursor =
xform * m_Precursor;
}
EcReal CircleJoint::jointParameterScalability() const
{
return 1.0;
}
//------------------------------------------------------------------------------
// XML Read / Write
//------------------------------------------------------------------------------
EcBoolean CircleJoint::read(
EcXmlReader& stream
)
{
EcBoolean result =
EcXmlCompoundType::read(stream);
m_PrimaryFrameInverse =
m_PrimaryFrame.inverse();
return result;
}
EcBoolean CircleJoint::write(
EcXmlWriter& stream
) const
{
return EcXmlCompoundType::write(stream);
}
//------------------------------------------------------------------------------
// Miscellaneous
//------------------------------------------------------------------------------
CircleJoint CircleJoint::nullObject()
{
return CircleJoint();
}
EcU32 CircleJoint::unitType() const
{
return LINEAR;
}
EcReal CircleJoint::upperBoundDhFrameDistance(
const EcJointActuator& jointActuator
) const
{
return EcMath::maximum(
calculateTransform(
jointActuator.upperLimit()
).translation().mag(),
calculateTransform(
jointActuator.lowerLimit()
).translation().mag()
);
}
void CircleJoint::changeInboardFrameBy(
const EcCoordinateSystemTransformation& offset
)
{
m_Precursor *= offset;
}
//------------------------------------------------------------------------------
// Self Test
//------------------------------------------------------------------------------
EcBoolean CircleJoint::selfTest() const
{
CircleJoint original;
original.setRadius(200.0);
const EcString filename =
"CircleJointSelfTest.xml";
EcXmlObjectReaderWriter::writeToFile(
original,
filename
);
CircleJoint loaded;
EcXmlObjectReaderWriter::readFromFile(
loaded,
filename
);
if (!(original == loaded))
{
EcPrint(Error)
<< "CircleJoint 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.