Blog

Implementing Custom Link Kinematics in Energid’s Actin SDK

Implementing Custom Link Kinematics in Energid’s Actin SDK
Sophia Dolan
Sophia Dolan
02/22/2019
0 Comments

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.

?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
#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.

?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
/////////////////////////////////////////////////////////////////////////
// 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.

?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
/////////////////////////////////////////////////////////////////////////
// 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.

?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
#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.

?
1
2
3
4
5
6
7
8
9
10
11
12
13
/////////////////////////////////////////////////////////////////////////
// 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.

?
1
ECXML_DEFINE_TOKENS(CircleJoint, CircleJointNamespace::CircleJointToken)

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

?
1
2
3
4
5
6
7
8
9
10
11
<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.

?
1
2
3
4
5
EcXmlObject* CircleJoint::libraryAttribute()
{
    m_LibraryAttribute = "CircleJoint.ecp";
    return &m_LibraryAttribute;
}

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

?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
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.

?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
//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.

?
1
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

?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
// ----------------------------------------------------------------------------------
// 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

?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
// ----------------------------------------------------------------------------------
// 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

?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
// ----------------------------------------------------------------------------------
// 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.

Related Blog Posts

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:

loading