DQ Robotics - arXiv
-
Upload
khangminh22 -
Category
Documents
-
view
1 -
download
0
Transcript of DQ Robotics - arXiv
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 1
DQ Robotics a Library for Robot Modeling andControl
Bruno Vilhena Adorno Senior Member IEEE and Murilo Marques Marinho Member IEEE
AbstractmdashDual quaternion algebra and its application torobotics have gained considerable interest in the last two decadesDual quaternions have great geometric appeal and easily capturephysical phenomena inside an algebraic framework that is usefulfor both robot modeling and control Mathematical objects suchas points lines planes infinite cylinders spheres coordinatesystems twists and wrenches are all well defined as dualquaternions Therefore simple operators are used to representthose objects in different frames and operations such as innerproducts and cross products are used to extract useful geometricrelationships between them Nonetheless the dual quaternionalgebra is not widespread as it could be mostly because efficientand easy-to-use computational tools are not abundant and usuallyare restricted to the particular algebra of quaternions Tobridge this gap between theory and implementation this paperintroduces DQ Robotics a library for robot modeling and controlusing dual quaternion algebra that is easy to use and intuitiveenough to be used for self-study and education while beingcomputationally efficient for deployment on real applications
I INTRODUCTION
DUAL QUATERNION ALGEBRA and its application torobotics have gained considerable interest in the last two
decades Far from being an abstract mathematical tool dualquaternions have great geometric appeal and easily capturephysical phenomena inside an algebraic framework that isuseful for both robot modeling and control Mathematical ob-jects such as points lines planes infinite cylinders spherescoordinate systems twists and wrenches are all well definedas dual quaternions Therefore simple operators are used torepresent those objects in different coordinate systems andoperations such as inner products and cross products areused to extract useful geometric relationship between themSome authors consider the particular set of dual quaternionswith unit norm known as unit dual quaternions as the mostefficient and compact tool to describe rigid transformations[1] [2] For instance homogeneous transformation matrices(HTM) have sixteen elements whereas dual quaternions haveeight elements and dual quaternion multiplications are less
This work was supported by CNPq CAPES FAPEMIGBV Adorno is with the Federal University of Minas Gerais (UFMG)
Department of Electrical Engineering 31270-010 Belo Horizonte-MG Brazile-mail adornoufmgbr B V Adorno is supported by CNPq Grant Numbers4240112016-6 and 3039012018-7
M M Marinho is with the University of Tokyo (UTokyo) Department ofMechanical Engineering Tokyo Japan e-mail murilonmltu-tokyoacjpM M Marinho is supported by JSPS KAKENHI Grant Number 19K14935
copy2020 IEEE Personal use of this material is permitted Permission fromIEEE must be obtained for all other uses in any current or future mediaincluding reprintingrepublishing this material for advertising or promotionalpurposes creating new collective works for resale or redistribution to serversor lists or reuse of any copyrighted component of this work in other works
expensive than HTM multiplications [3 p 42] Moreover itis easy to extract geometric parameters from a given unit dualquaternion (translation axis of rotation angle of rotation)Nonetheless they are easily mapped into a vector structurewhich can be particularly convenient to perform tasks such aspose control as there is no need to extract parameters from thedual quaternion
Nonetheless the dual quaternion algebra is not widespreadas it could be not only because classic matrix algebra on realnumbers is very mature and it is the backbone of most roboticstextbooks [4]ndash[6] but also because efficient and easy-to-usecomputational tools are not abundant and usually are restrictedto the particular algebra of quaternions Indeed the BoostMath library1 implements quaternions and even octonions butnot dual quaternions whereas the Eigen2 library implementsonly quaternions both in C++ language Some libraries alsoimplement dual quaternions in Lua3 MATLAB [7] and C++4
but none of them are focused on roboticsThis paper introduces DQ Robotics a library for robot
modeling and control using dual quaternion algebra that iscomputationally efficient easy to use and is intuitive enoughto be used for self-study and education and sufficientlyefficient for deployment on real applications For instanceDQ Robotics has already been used in real platforms suchas cooperative manipulators for surgical applications mobilemanipulators and humanoids among several other roboticsystems some of which are shown in Fig 10 It is writtenin three languages namely Python MATLAB and C++ allof them sharing a unified programming style to make thetransition from one language to another as smooth as possibleenabling fast prototype-to-release cycles Furthermore a greateffort has been made to make coding as close as possible tothe mathematical notation used on paper making it easy toimplement code as soon as one has grasped the mathematicalconcepts
DQ Robotics uses the expressiveness of dual quaternionalgebra for both robot modeling and control This paperintroduces the main features of the library as well as its basicusage in a tutorial-like style The tutorial begins with thepresentation of dual quaternion notation and basic operationsgoes through robot modeling and control and ends with acomplete robot control example of two robots cooperating ina task where one manipulator robot and a mobile manipulatorinteract while deviating from obstacles in the workspace
1httpswwwboostorg2httpeigentuxfamilyorg3httpsesslabjp~essencodelibdq4httpsglmg-trucnet
arX
iv1
910
1161
2v3
[cs
RO
] 2
4 Ju
l 202
0
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 2
A How to follow the tutorial
In this paper for brevity we present several code snippets inMATLAB language that should be familiar to most roboticistsSuitable commands are also available in Python and C++The readers might be interested in downloading and installingthe DQ Robotics toolbox5 for MATLAB and trying out thecode snippets in their own machine More details are given inSection VIII and in the DQ Robotics documentation6
The tutorial also has a few diagrams using the UnifiedModeling Language (UML) which we use to explain theobject-oriented modeling of the library in a way that isagnostic to the programming language A simplified UMLdiagram that shows the relevant object-oriented concepts usedin this paper is briefly explained in Fig 1
Fig 1 A simplified class diagram in the Unified Modeling Language (UML)Abstract methods and classes are italicized Concrete methods and classes arein upright text Static method are underlined Specific arrowheads indicateinheritance and aggregation
B Notation choice and common operations
Given the imaginary units ık that satisfy ı2 = 2 = k2 =ık = minus1 and the dual unit ε which satisfies ε 6= 0 andε2 = 0 the quaternion set is defined as
H h1 + ıh2 + h3 + kh4 h1 h2 h3 h4 isin R
and the dual quaternion set is defined as
H h1 + εh2 h1h2 isin H
There are several equivalent notations for representing dualquaternions but we adopt the hypercomplex one which re-gards them as an extension of complex numbers This waysums multiplications and subtractions are exactly the sameused in complex numbers (and also in real numbers) differ-ently from the scalar-plus-vector notation which requires theredefinition of the multiplication operation When designingDQ Robotics one important guiding principle was to be ableto use dual quaternion operations as close as possible to theway that we do it on paper closing the gap between theoryand implementation
For instance the dual quaternions a = ı + ε(1 + k
)and
b = minus2 + + ε(ı+ k
)are declared in MATLAB as
a = DQi + DQE(1 + DQk)b = -2 + DQj + DQE(DQi + DQk)
5httpsgithubcomdqroboticsmatlabreleaseslatest6httpsdqroboticsgithubioreadthedocsioenlatestinstallationhtml
and
gtgt a + bans = ( - 2 + 1i + 1j) + E(1 + 1i + 2k)gtgt a bans = ( - 2i + 1k) + E( - 3 - 1i - 2k)
Alternatively one can use include_namespace_dq to enablethe aliases i_ j_ k_ and E_ Therefore
gtgt E_aans = E(1i)
The list of the main DQ class operations is shown in Table I
TABLE IMAIN METHODS OF CLASS DQ
Binary operations between two dual quaternions (eg a + b)+ - Sum subtraction and multiplication Right division and left division
Unary operations (eg -a and arsquo)- Minus[rsquo] [rsquo] Conjugate and sharp conjugate
Unary operators (eg P(a))conj sharp Conjugate and sharp conjugateexp log Exponential of pure dual quaternions and logarithm
of unit dual quaternionsinv Inverse under multiplicationhamiplus4haminus4
Hamilton operators of a quaternion
hamiplus8haminus8
Hamilton operators of a dual quaternion
norm Norm of dual quaternionsP D Primary part and dual partRe Im Real component and imaginary componentsrotation Rotation component of a unit dual quaternionrotation_axisrotation_angle
Rotation axis and rotation angle of a unit dualquaternion
vec3 vec6 Three-dimensional and six-dimensional vectorscomposed of the coefficients of the imaginary part ofa quaternion and a dual quaternion respectively
vec4 vec8 Four-dimensional and eight-dimensional vectorscomposed of the coefficients of a quaternion and adual quaternion respectively
Binary operators (eg Ad(ab))Ad Adsharp Adjoint operation and sharp adjointcross dot Cross product and dot product
Binary relations (eg a == b)== ~= Equal and not equal
Common methods[plot] Plots coordinate systems lines and planes
Methods and operations enclosed by brackets (eg []) are available onlyon MATLAB
II REPRESENTING RIGID MOTIONS
Since the set of dual quaternions represents an eight-dimensional space it is particularly useful to represent ro-tations translations and more generally rigid motions Forinstance the set S3 h isin H h = 1 is used to representrotations with h
radichhlowast =
radich21 + h22 + h23 + h24 being
the quaternion norm and hlowast = h1minus(h2 ı+h3+h4k) being theconjugate of h = h1 + h2 ı+ h3+ h4k Any element r isin S3can always be written as r = cos (φ2) + n sin (φ2) wheren = nx ı+ ny + nz k isin S3 is the unit-norm rotation axis andφ isin [0 2π) is the rotation angle Therefore a rotation of π radaround the x-axis is given by rπx cos(π2)+ ı sin(π2) =ı
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 3
Since the multiplication of unit quaternions is also a unitquaternion a sequence of rotations is given by a sequence ofmultiplications of unit quaternions For instance if first weperform the rotation rπx and then we use the rotated frameto do another rotation of π rad around the y-axis given byrπy cos(π2) + sin(π2) = then the final rotation isgiven by rπxrπy = ı = k because ık = minus1 and k2 = minus1Besides because k = cos(π2)+ k sin(π2) then rπxrπy =rπz which represents the rotation of π rad around the z-axisThis is illustrated in Fig 2
Fig 2 Sequence of rotations represented by multiplication of unit quater-nions Each arrow has the same color of the rotation axis
The set of pure quaternions is defined as Hp h1 ı+ h2+ h3k h1 h2 h3 isin R
sub H and is used to
represent elements of a tridimensional space For instance thepoint (x y z) can be directly mapped to xı+ y+ zk whichis an element of Hp Furthermore given a point p0 isin Hp inframe F0 and the unit quaternion r1 isin S3 that represents therotation from F0 to F1 the coordinates of the point in F1 isp1 = rlowast1p
0r1 Analogously given r2 isin S3 that represents therotation from F1 to F2 the coordinates of the point in F2
is p2 = rlowast2p1r2 = rlowast2r
lowast1p
0r1r2 which implies that the unitquaternion that represent the rotation from F0 to F2 is givenby r1r2 showing again that a sequence of rotations is givenby multiplication of unit quaternions
Translations and rotations can be grouped into a singleunit dual quaternion More specifically a rigid motion isrepresented by
x = r + ε1
2pr (1)
as illustrated in Fig 3 and a sequence of rigid motions isgiven by multiplication of unit dual quaternions For a moredetailed description of the fundamentals of dual quaternionalgebra see [8]
Using DQ Robotics a translation of (01 02 03)m fol-lowed by a rotation of π rad around the y-axis using (1) isdeclared in MATLAB as
Listing 1 Declaring a unit dual quaternion in MATLABr = cos(pi2) + j_sin(pi2)p = 01i_ + 02j_ + 03k_xd = r + E_05pr
Alternatively one could declare the rotation and translation as
Fig 3 A rigid motion x between frames Fa and Fb given by a translationp followed by a rotation r
r = DQ([cos(pi2) 0 sin(pi2) 0])p = DQ([0 01 02 03])
Whereas the first way is closer to the mathematical notationthe second one can be convenient when using the matrixfunctions available in MATLAB Python and C++
III ROBOT KINEMATICS
The implementation of robot kinematic modeling is doneby means of the abstract class DQ_Kinematics and all itssubclasses whose hierarchy is shown in Fig 4 The librarycurrently supports serial manipulators mobile bases and theircomposition which results in mobile manipulators (managedby the DQ_WholeBody class) bimanual systems (managedby the DQ_CooperativeDualTaskSpace class) and evenbranched mechanisms such as bimanual mobile manipulatorsCommon manipulator robots are available such as the KUKALWR4 and the Barrett WAM Arm but custom robots can beeasily created by using arbitrary Denavit-Hartenberg parame-ters Some common mobile robots such as the differential-drive iRobot Create are also available Creating new onesrequires only the wheels radiuses and the axis length
Let us consider the KUKA YouBot which is a mobile ma-nipulator composed of a 5-DOF manipulator serially coupledto a holonomic base One easy way of defining it on DQRobotics is to model the arm and the mobile base separatelyand then assemble them to form a mobile manipulator Theserial arm is defined with the help of the Denavit-Hartenbergparameters as followsarm_DH_theta = [0 pi2 0 pi2 0]arm_DH_d = [0147 0 0 0 0218]arm_DH_a = [0 0155 0135 0 0]arm_DH_alpha = [pi2 0 0 pi2 0]arm_DH_matrix = [arm_DH_theta arm_DH_d arm_DH_a
arm_DH_alpha]arm = DQ_SerialManipulator(arm_DH_matrix rsquostandardrsquo)
The holonomic base is defined asbase = DQ_HolonomicBase()
and then they are coupled together in a DQ_WholeBody objectx_bm = 1 + E_05(022575i_ + 01441k_)baseset_frame_displacement(x_bm)robot = DQ_WholeBody(base)robotadd(arm)
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 4
Since there is a displacement between the mobile base frameand the location where the arm is attached we use the methodset_frame_displacement() New kinematic chains can beserially coupled to the last chain by using the method add()
Because the KUKA Youbot is already defined in DQRobotics it suffices to instantiate an object of the KukaYoubotclass
youbot = KukaYoubotkinematics()
All standard robots in DQ Robotics are defined inside thefolder [root_folder]robots and have the static methodkinematics() that returns a DQ_Kinematics object There-fore a KUKA LWR 4 robot manipulator can be definedanalogously
lwr4 = KukaLwr4Robotkinematics()
All DQ_Kinematics subclasses have common functions forrobot kinematics such as the ones used to calculate the forwardkinematics the Jacobian matrix that maps the configurationvelocities to the time-derivative of the end-effector pose aswell as other Jacobian matrices that map the configurationvelocities to the time derivative of other geometrical primitivesattached to the end-effector such as lines and planes Thisincludes DQ_WholeBody objects therefore the correspondingJacobians are whole-body Jacobians that take into account thecomplete kinematic chain The list of the main methods issummarized in the simplified UML diagram in Fig 4 anddetailed in Table II
IV GEOMETRIC PRIMITIVES
Several geometrical primitives such as points planes andlines are represented as elements of the dual quaternion algebra[8] which is particularly useful when incorporating geometricconstraints into robot motion controllers For instance a planeexpressed in a frame Fa is given by πa = na + εda wherena = nx ı + ny + nz k is the normal to the plane andda = 〈pana〉 is the signed distance between the plane andthe origin of Fa and pa is an arbitrary point on the plane asillustrated in Fig 5
Therefore the plane πa = + ε15 (ie a plane whosenormal is parallel to the y-axis and whose distance from Fais 15 m) is defined in DQ Robotics as
plane_a = j_ + E_ 15
Analogously a line in Fa with direction given by la = lx ı+ly + lz k and passing through point sa = px ı+ py + pz k isrepresented in dual quaternion algebra as la = la + εsa times laas illustrated in Fig 5 Therefore a line parallel to the y-axispassing through point sa = ı (ie with coordinates (1 0 0))is defined in DQ Robotics as
line_a = j_ + E_ cross(i_j_)
In MATLAB the plot functions are available for framesplanes and lines
plot(DQ(1)rsquonamersquo rsquo$mathcalF_a$rsquo)plot(plane_a rsquoplanersquo 20 rsquocolorrsquo rsquomagentarsquo)plot(line_a rsquolinersquo 5)
Fig 4 Simplified UML class diagram of the robot modeling classes with themain methods that are available in C++ Python and MATLAB
The result is shown in Fig 6The library implements the utility class DQ_Geometry
which provides methods for geometric calculations be-tween primitives such as distances Jacobians relating therobot joint velocities to primitives attached to the end-effector are available in the DQ_Kinematics class suchas line_jacobian() and plane_jacobian() In addi-tion DQ_Kinematics contains several Jacobians that re-late the time derivative of the distance between two primi-tives (eg plane_to_point_distance_jacobian()) whichare grouped in the UML class diagram of Fig 4 asprim_dist_jacobian()
V ROBOT MOTION CONTROL
Several advantages make dual quaternion algebra attractivewhen designing controllers On the one hand unit dual quater-nions are easily mapped into a vector structure The vectorstructure is particularly convenient in pose control as there isno need to use intermediate mappings nor extract parameters
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 5
TABLE IIMAIN METHODS OF CLASS DQ_Kinematics AND ITS SUBCLASSES
DQ_Kinematicsfkm Compute the forward kinematicspose_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of the pose of a frame attached to
the robotset_base_frame Set the physical location of the robot in spaceset_reference_frame Set the reference frame for all calculationsline_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of a line attached to the robotplane_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of a plane attached to the robotrotation_jacobian Compute only the rotational part of the pose_jacobiantranslation_jacobian Compute only the translational part of the pose_jacobianprim_dist_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of the distance between a
primitive attached to the robot and another primitive in the workspace For instance prim_dist_jacobian can beline_to_line_distance_jacobian line_to_point_distance_jacobianplane_to_point_distance_jacobian point_to_line_distance_jacobianpoint_to_plane_distance_jacobian point_to_point_distance_jacobian
DQ_SerialManipulatorpose_jacobian_derivative Compute the analytical time derivative of the pose Jacobian matrixset_effector Set a constant transformation for the end-effector pose with respect to the frame attached to the end of the last link
DQ_MobileBase DQ_HolonomicBase and DQ_DifferentialDriveRobotset_frame_displacement Set the rigid transformation for the base frameset_base_diameter Change the base diameterconstraint_jacobian Compute the Jacobian that relates the wheels velocities to the configuration velocities
DQ_WholeBodyadd Add a new element to the end of the serially coupled kinematic chainadd_reversed Add a new element but in reverse order to the end of the serially coupled kinematic chainget_chain Returns the complete kinematic chainsequential Reorganize a sequential configuration vector in the ordering required by each kinematic chain (ie the vector blocks
corresponding to reversed chains are reversed)
DQ_CooperativeDualTaskSpaceabsolute_pose Compute the pose of a frame between the two end-effectors which is usually related to a grasped objectabsolute_pose_jacobian Compute the Jacobian that maps the joint velocities of the two-arm system to the time derivative of the absolute posepose1 and pose2 Compute the poses of the first and second end-effectors respectivelypose_jacobian1 andpose_jacobian2
Compute the Jacobians that maps the configurations velocities to the time derivative of the poses of the first andsecond end-effectors respectively
relative_pose Compute the rigid transformation between the two end-effectorsrelative_pose_jacobian Compute the Jacobian that maps the joint velocities of the two-arm system to the time derivative of the relative pose
Abstract methods are written in italics concrete methods are written in upright and static methods are underlined Concrete methods that implement theirabstract counterparts are omitted for the sake of conciseness
Fig 5 Two geometric primitives represented by elements of the dualquaternion algebra (a) the blue plane πa represented by the normal na
and the distance to the origin da = 〈pana〉 where pa is an arbitrary pointon the plane (b) and the green line la represented by the direction la andthe moment sa times la where sa is an arbitrary point on the line
Fig 6 MATLAB plot reference frame Fa line la = + ε(ıtimes ) parallelto the y-axis and passing through point (1 0 0) and plane πa = + ε15with normal parallel to the y-axis and whose distance from the origin of Fa
is 15 m
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 6
from the dual quaternion On the other hand the position iseasily extracted from the HTM but to obtain the orientationone usually has to extract the rotation angle and the rotationaxis from the rotation matrix This extraction introduces rep-resentational singularities when the rotation angle equals zeroFurthermore when designing constrained controllers based ongeometrical constraints one has to represent several geometri-cal primitives in different coordinate systems and in differentlocations Dual quaternion algebra is particularly useful in thiscase because several relevant geometrical primitives are easilyrepresented as dual quaternions as shown in Section IV
Users can implement their own controllers in DQ RoboticsFor instance suppose that the desired end-effector pose isgiven by Listing 1 and the initial robot configuration andcontroller parameters are given in Listing 2
Listing 2 Parameters for a simple kinematic controllerq = [0 03770 01257 -05655 0 0 0]rsquoT = 0001 sampling timegain = 10 controller gain
A classic controller based on an Euclidean error function andthe Jacobian pseudo-inverse can be implemented in MATLABas shown in Listing 3
Listing 3 Kinematic controller based on the Jacobian pseudoinversee = ones(81) initialize the error vectorwhile norm(e) gt 0001
J = lwr4pose_jacobian(q)x = lwr4fkm(q)e = vec8(x-xd)u = -pinv(J)gaineq = q + Tu
end
In Listing 3 when executing the controller on a real robot thecontrol input u is usually sent directly to the robot
There are several kinematic controllers included in DQRobotics including the one in Listing 3 The class hierarchyfor the kinematic controllers is summarized by the simplifiedUML diagram in Fig 7 All kinematic controller classes inheritfrom the abstract superclass DQ_KinematicController andtheir main methods are detailed in Table III
The DQ_KinematicController subclasses are focused onthe control structure not on particular geometrical tasks Thesame subclass can be used regardless if the goal is to controlthe end-effector pose position orientation etc In order todistinguish between task objectives it suffices to usecontrollerset_control_objective(GOAL)
where GOAL is an object from the enumeration classControlObjective which currently provides the follow-ing enumeration members Distance DistanceToPlaneLine Plane Pose Rotation and Translation Thereforethe implementation in Listing 3 can be rewritten using theDQ_PseudoinverseController class as
Listing 4 Controller based on the DQ_PseudoinverseController classcontrol = DQ_PseudoinverseController(lwr4)controlset_control_objective(ControlObjectivePose)
controlset_gain(gain)controlset_stability_threshold(00001)while ~controlsystem_reached_stable_region()
Fig 7 Simplified UML class diagram of the robot kinematic controller classeswith the main methods that are available in C++ Python and MATLAB
u = controlcompute_setpoint_control_signal(qvec8(xd))q = q + Tu
end
Furthermore if the geometric task objective changes theDQ_KinematicController subclass is responsible for calcu-lating the appropriate forward kinematics and the correspond-ing Jacobian For instance if a line is attached to the end-effector and the goal is to align it with a line in the workspacethen it suffices to replace the second line of Listing 4 bycontrolset_primitive_to_effector(line)controlset_control_objective(ControlObjectiveLine)
where line is a dual quaternion corresponding to a linepassing through the origin of the end-effector frame
Furthermore since any subclass ofDQ_KinematicController provides the methods usedin Listing 4 changing the controller is a matter of changingjust one line of code namely the first one in Listing 4
VI INTERFACE WITH ROSThe Robot Operating System (ROS) [9] is widely used
by the robotics community After the one-line installationdescribed in Section VIII the Python version of the DQRobotics library can be imported in any Python script in thesystem therefore it is compatible with ROS out-of-the-boxMATLAB also has an interface with ROS7
For C++ code in its most recent versions ROS uses thecatkin_build8 environment which itself depends on CMAKEGiven that DQ Robotics C++ is installed as a system library inUbuntu as shown in Section VIII ROS users can readily haveaccess to the library as they would to any other system librarywith a proper CMAKE configuration After installation theC++ version of the library can be linked with
7httpswwwmathworkscomhelprosugget-started-with-roshtml8httpscatkin-toolsreadthedocsioenlatestverbscatkin_buildhtml
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 7
TABLE IIIMAIN METHODS OF CLASS DQ_KinematicController AND ITS MAIN SUBCLASSES
DQ_KinematicControllercompute_setpoint_control_signal Compute the control input to regulate to a set-pointcompute_tracking_control_signal Compute the control input to track a trajectoryget_control_objective Return the control objectiveget_jacobian Return the correct Jacobian based on the control objectiveget_task_variable Return the task variable based on the control objectiveis_set Verify if the controller is set and ready to be usedset_control_objective Set the control objective using predefined goals in ControlObjectiveset_damping Set the damping to prevent instabilities near singular configurationsset_gain Set the controller gainset_primitive_to_effector Attach primitive (eg plane line point) to the end-effectorset_stability_threshold Set the threshold that determines if a stable region has been reachedsystem_reached_stable_region Return true if the trajectories of the closed-loop system have reached a stable region (ie a
positive invariant set) false otherwiseverify_stability Verify if the closed-loop region has reached a stable region
DQ_PseudoinverseSetpointControllercompute_tracking_control_signal Given the task error x = xminus xd isin Rn and the feedforward term xd compute the control
signal u = J+ (minusλx+ xd) where J+ is the pseudoinverse of the task Jacobian J and λis the controller gain
DQ_KinematicConstrainedControllerset_equality_constraint Add equality constraints of type Bq = b where q isin Rn is the vector of joint velocities
b isin Rm is the vector of equality constraints and B isin Rmtimesnset_inequality_constraint Add inequality constraints of type Aq a where q isin Rn is the vector of joint velocities
a isin Rm is the vector of inequality constraints and A isin Rmtimesn
DQ_TaskspaceQuadraticProgrammingControllercompute_objective_function_linear_component Compute the vector h used in the objective function (12)qTHq + hT qcompute_objective_function_symmetric_matrix Compute the matrix H used in the objective function (12)qTHq + hT qcompute_tracking_control_signal Given the task error x = xminus xd isin Rn compute the control signal given by
u isin argminq(12)qTHq + hT q subject to Aq a and Bq = b
DQ_ClassicQPControllercompute_objective_function_linear_component Compute the vector h used in (12)qTHq + hT q = Jq + λxminus xd22compute_objective_function_symmetric_matrix Compute the matrix H used in (12)qTHq + hT q = Jq + λxminus xd22
Abstract methods are written in italics and concrete methods are written in upright In all concrete classes the methodcompute_setpoint_control_signal() does not include the feedforward term in the method parameters and is equivalent to the methodcompute_tracking_control_signal with xd = 0 Therefore they are omitted for the sake of conciseness
target_link_libraries(my_binary dqrobotics)
for a given binary called my_binary
VII INTERFACE WITH V-REPDQ Robotics provides a simple interface to V-REP [10]
enabling users to develop complex simulations without havingto delve into the V-REP documentation The V-REP interfacecomes bundled with the MATLAB and Python versions of thelibrary The C++ version of our V-REP interface comes asa separate package and the following CMAKE directive willlink the required shared objecttarget_link_libraries(my_binary
dqroboticsdqrobotics -vrep-interface)
for a given my_binaryThe available methods are shown in Table IV
To start the communication with V-REP using its remoteAPI9 only two methods are necessaryvi = DQ_VrepInterfaceviconnect(rsquo127001rsquo19997)vistart_simulation()
9httpwwwcoppeliaroboticscomhelpFilesenlegacyRemoteApiOverviewhtm
TABLE IVVrepInterface
connect Connect to a V-REP Remote API Serverdisconnect Disconnect from currently connected serverdisconnect_all Flush all Remote API connectionsstart_simulation Start V-REP simulationstop_simulation Stop V-REP simulationget_object_translation Get object translation as a pure quaternionset_object_translation Set object translation with a pure quaternionget_object_rotation Get object rotation as a unit quaternionset_object_rotation Set object rotation with a unit quaternionget_object_pose Get object pose as a unit dual quaternionset_object_pose Set object pose with a unit dual quaternionset_joint_positions Set the joint positions of a manipulator robotset_joint_target_positions Set the joint target positions of a manipulator
robotget_joint_positions Get the joint positions of a manipulator robot
The method start_simulation starts the V-REP simulationwith the default asynchronous mode and the recommended 5ms communication thread cycle10
Since each robot joint in V-REP is associated with a nameit is convenient to encapsulate this information inside the robotclass Therefore classes implementing robots that use the V-
10For other parameters refer to the code
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 8
REP interface must be a subclass of DQ_VrepRobot to pro-vide the methods send_q_to_vrep and get_q_from_vrepwhich are used to send the desired robot configuration vectorto V-REP and get the current robot configuration vector fromV-REP respectively
Objects poses in a V-REP scene can be retrievedor set by using the methods get_object_pose andset_object_pose respectively which are useful when de-signing motion planners or controllers that take into accountthe constraints imposed by obstacles Finally in order to endthe V-REP simulation it suffices to use two methodsvistop_simulation()vidisconnect()
A A more complete example
To better highlight how DQ Robotics can be used withV-REP in this example a KUKA LWR4 manipulator robotinteracts with a KUKA YouBot mobile manipulator in aworkspace containing three obstacles namely two cylindersand a plane as shown in Fig 8
Fig 8 A more complete example The reference frame is given by F thetime-varying frame Fc is used to indicate where the circle must be drawn inthe whiteboard and Fmm is the end-effector frame of the mobile manipulatorObstacles are represented as geometrical primitives the wall is representedby the plane π = nπ + ε〈pπ nπ〉 where pπ is an arbitrary point onthe plane and nπ is the plane normal and the two cylindrical obstaclesare represented by (l1 R1) and (l2 R2) where li = li + εpi times li isthe ith cylinder centerline and Ri is the corresponding cylinder radius withi isin 1 2 Furthermore li is the line direction and pi is an arbitrary pointon the ith line To prevent collisions between the robot and the obstacles therobot is represented by a circle given by (p3 R3) where p3 and R3 are thecenter and radius of the circle respectively
The manipulator robot holds a whiteboard and uses apseudo-inverse-based controller with a feedforward term totrack a trajectory given by xm(t) = rm(t)xm(0)pm(t)where rm = cos(φ(t)2) + k sin(φ(t)2) with φ(t) =(π2) sin(ωnt) and p = 1 + (12)εdz cos(ωdt)k Viewedfrom the top the whiteboard will follow a semi-circular pathwith a radial oscillation at a frequency of ωd rads andamplitude dzm and an oscillation around the vertical axisat a frequency of ωn rads The mobile manipulator holds afelt pen and follows the manipulator trajectory while drawing a
circle on the whiteboard The mobile manipulator desired end-effector trajectory is given by xmm(t) = xm(t)xc wherexc = 1 + (12)ε0015k is a constant displacement of 15cmalong the z-axis to account for the whiteboard width and = cos(π2) + sin(π2) is a rotation of π around they-axis so that both end-effectors have their z-axis pointingto opposite directions To prevent collision with obstaclesit uses a constrained controller in which for each obstaclewe use a differential inequality to ensure that the robot willapproach it in the worst case with an exponential velocitydecrease For each obstacle we define a distance metricd (t) d (t)minus dsafe where dsafe isin [0infin) is an arbitraryconstant safe distance and the following inequalities must holdfor all t
˙d (t) ge minusηdd (t)lArrrArr minusJdq le ηdd (t) (2)
where ηd isin [0infin) is used to adjust the approach velocity andJd is the distance Jacobian related to the obstacle [11] Thelower is ηd the lower is the allowed approach velocity
In the case of our example to prevent collisions betweenthe robot and the wall we let dsafe R3 (see Fig 8) suchthat d (t) = 0 lArrrArr d (t) = R3 and the distance function isdefined as the distance between p3 which is the center of thecircle enclosing the robot and the wall plane π Analogouslyto prevent collisions between the robot and the static cylinderswe use one inequality such as (2) for each cylinder wheredsafe Ri + R3 and d(t) is defined as the distance betweenp3 and the lines li for i isin 1 2 Although those distancefunctions and corresponding Jacobians can be computed byusing the classes DQ_Geometry and DQ_Kinematics respec-tively details of how they are obtained using dual quaternionalgebra are presented in [11]
Whenever the felt-tip is sufficiently close to the whiteboardit starts to draw a circle Figs 9a and 9b show that the mobilemanipulator successfully draws the circle although there aresome small imperfections as the obstacle avoidance is a hardconstraint that is always respected at the expense of worsetrajectory tracking Last Fig 9c shows the mobile manipulatortrajectory which is always adapted to prevent collisions withthe cylinders and the wall
The main MATLAB code is shown in Listing 5 For the sakeof conciseness the functions compute_lwr4_referenceand compute_youbot_reference which calculate theaforementioned end-effector trajectories are omittedThe same applies for get_plane_from_vrep andget_line_from_vrep which get information about thelocation of the corresponding geometric primitives inV-REP and compute_constraints which calculatesconstraints such as (2) for each obstacle in the scene Theoriginal source code used in this example is available assupplementary material For a more up-to-date version referto httpsgithubcomdqroboticsmatlab-examples
VIII DEVELOPMENT INFRASTRUCTURE
Aiming at the scalability of the DQ Robotics library andtaking advantage of the familiarity that current developers havewith Git and Github we opted to also use those services
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 9
(a) Circle partially drawn at t = 36s
(b) Circle completely drawn at t = 170s
(c) Top view showing the mobile manipulator trajectory
Fig 9 Manipulator robot interacting with a mobile manipulator in aworkspace with two cylindrical obstacles and a wall The parameters for thetrajectory generation were ωn = 01 ωd = 05 and dz = 01
End-users are agnostic to these infrastructural decisions Forthem usage performance and installation of the library matterthe most The usage of the library has been simplified byrelying on a programming syntax similar to the mathemati-
Listing 5 Main MATLAB code for the simulationsampling_time = 005total_time = 200for t=0sampling_timetotal_time
Get obstacles from V-REPplane = get_plane_from_vrep(virsquoObstaclePlanersquoDQk)cylinder1 = get_line_from_vrep(virsquoObstacleCylinder1rsquoDQk)cylinder2 = get_line_from_vrep(virsquoObstacleCylinder2rsquoDQk)
Set references for both robots[lwr4_xd lwr4_ff] = compute_lwr4_reference(lwr4
simulation_parameters lwr4_x0 t)[youbot_xd youbot_ff] = compute_youbot_reference(
youbot_control lwr4_xd lwr4_ff)
Compute the control input for the manipulatorlwr4_u = lwr4_controllercompute_tracking_control_signal(
lwr4_q vec8(lwr4_xd)vec8(lwr4_ff))
Compute constrained control input for the youbot[Jconstraint bconstraint] = compute_constraints(youbot
youbot_q planecylinder1 cylinder2)youbot_controlset_inequality_constraint(-Jconstraint
1bconstraint)youbot_u= youbot_controlcompute_tracking_control_signal(
youbot_q vec8(youbot_xd) vec8(youbot_ff))
Since we are using V-REP just for visualization integrate the control signal to update the robots configurationslwr4_q = lwr4_q + sampling_timelwr4_uyoubot_q = youbot_q + sampling_timeyoubot_u
Send desired values to V-REPlwr4_vreprobotsend_q_to_vrep(lwr4_q)youbot_vreprobotsend_q_to_vrep(youbot_q)end
cal description and being attentive to proper object-orientedprogramming practices Furthermore good performance isachieved by careful optimization of the library and by theC++ implementation
The ease of installation has been addressed in language-platform specific ways that we document in details on the DQRoboticsrsquo Read the Docs11
For MATLAB DQ Robotics is distributed as a Toolboxthat can be installed by the end-user in any of the MATLAB-compatible operating systems
For C++ we officially focus on Ubuntu long-term service(LTS) distributions in a similar way to the most recentdistributions of ROS and other large open-source libraries suchas TensorFlow12 For any Ubuntu LTS distribution that has notreached its end-of-life13 the user can install the C++ versionof the library with the following three commandssudo add-apt-repository ppadqrobotics -devreleasesudo apt-get updatesudo apt-get install libdqrobotics
made available via our release Personal Package Archive(PPA)14 in which we store the latest stable versions of thelibrary Interfaces between DQ Robotics and other librariesalso reside in the same PPA For example after addingthe PPA the user can install the V-REP interface with thefollowing commandsudo apt-get install libdqrobotics -interface -vrep
11httpsdqroboticsgithubioreadthedocsioenlatestinstallationhtml12httpswwwtensorfloworg13httpsubuntucomaboutrelease-cycle14httpslaunchpadnet~dqrobotics-dev+archiveubunturelease
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 10
The PPA guarantees that the user can reliably install thepackage in their system as the PPA only stores packages thatcompiled successfully Support for other operating systemswill be driven by user interest but should not be a bigchallenge for most systems since we are using only CMAKEand Eigen3 as project dependencies both of which are widelysupported
Lastly the Python version of DQ Robotics is made avail-able through a Pybind11-based15 Python wrapper of the C++library This has three main advantages First there is no needto redevelop and maintain versions of the libraries in differentprogramming languages which is highly demanding Secondunit-testing code written in Python which is easier to writeand maintain automatically validates the C++ library as wellThird we can have Pythonrsquos ease-of-use with C++rsquos perfor-mance for each function Although there is a computationaloverhead when using the Python bindings (in contrast withdirectly using the C++ code) it is much lower than a nativePython implementation A simple example of this can be seenin Table V in which we compare the average required timefor dual quaternion multiplications in the same machine
TABLE VCOMPUTATIONAL TIME OF DUAL-QUATERNION MULTIPLICATIONS
Mean [micros] Standard Deviation [micros]C++ (gcc 540) 022 00149
MATLAB R2019a 894 01223Python 352 Bindings 082 00242Python 352 Native 3142 01646
Mean and standard deviation of the required time for one dual-quaternion multiplicationThis was calculated from a thousand sets of a thousand executions on the same IntelCore i9 9900K Ubuntu 1604 x64 system and excludes the time required to generate therandom dual quaternions
An Ubuntu LTS user can install the Python3 version of DQRobotics from the Python Package Index (PyPI)16 with a singleline of code
python3 -m pip install --user dqrobotics
Lastly the PyPI package is continuously built from sourceusing TravisCI17 so that the code is properly compiled andtested before being distributed to our users
IX APPLICATIONS AND COMPARISON WITH OTHERPACKAGES AND LIBRARIES
DQ Robotics has been used for more than nine years tomodel and control different robots in applications such asbimanual surgical robots in constrained workspaces (Fig 10a)whole-body control of humanoid robots (Fig 10b) andthe decentralized formation control of mobile manipulators(Fig 10c)18
The main difference from other packages and librariesis that DQ Robotics implements dual quaternion algebra in
15httpsgithubcompybindpybind1116httpspypiorg17httpstravis-cicom18A more complete list of papers and applications can be found in https
dqroboticsgithubiocitationshtml
(a) The bimanual surgical robot SmartArm [12]
(b) Whole-body control of a humanoid robot [13]
(c) Cooperative mobile manipulators [14]
Fig 10 Examples of different applications of DQ Robotics
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 11
a close way to the mathematical notation Furthermore allsupported languages namely Python MATLAB and C++use the same convention and style and it is straightforwardto port code in one language to other supported languages Toenable that we avoid using features that are not available in allthree languages unless that would lead to poor-quality codeFrom the user point-of-view the advantage of such approachis that users can prototype their ideas on MATLAB usingthe convenient plot system then easily translate the code toC++ and deploy it on a real robotic platform The Pythonversion of the library is somewhere between MATLAB andC++ being suitable for prototyping and also for deploymenton real applications because C++ runs under the hood
In terms of target audience the MATLAB version of DQRobotics has some overlap with Peter Corkersquos Robotics Tool-box [15] most notably in terms of their use for educationand learning On the one hand earlier versions were greatlyinspired by Peter Corkersquos Robotics Toolbox most notablythe plot system which is particularly useful when teachingrobotics On the other hand DQ Robotics is entirely basedon general dual quaternion algebra whereas Robotics Tool-box uses classic representations for robot modeling althoughit does offer support for unit quaternions Moreover otherfunctionalities of Peter Corkersquos Robotics Toolbox such aspath planning localization and mapping are not available inDQ Robotics as of now However some functionalities ofPeter Corkersquos Robotics Toolbox are now available in toolboxesdistributed by MathWorks such as the Image ProcessingToolbox19 and the Robotics System Toolbox20 which are inprinciple compatible with DQ Robotics
Another MATLAB toolbox [7] focuses on the application ofunit dual quaternions to the neuroscience domain and presentsbasic kinematic modeling of serial mechanisms In addition tohaving many fewer functionalities than DQ Robotics it usesprocedural programming which makes the code quite differentfrom the notation used on paper
A major contrast between DQ Robotics and Peter Corkersquosand Leclercqrsquos MATLAB toolboxes is that DQ Robotics hasPython and C++ versions with identical APIs as far as theprogramming languages permit This increases the numberof possible users and considerably reduces the time requiredto go from prototyping to deployment Moreover this makesDQ Robotics compatible with powerful libraries available inPython such as SciPy21 and scikit-learn22 In addition bothPython and C++ versions of the library can be readily usedalongside ROS which is the current standard of shareablecode in the robotics community Lastly a modern developmentinfrastructure not available in other existing libraries makesthe library scalable and easier to maintain and extend
X CONCLUSIONS
This paper presented DQ Robotics a comprehensive com-putational library for robot modeling and control using dual
19httpswwwmathworkscomproductsimagehtml20httpswwwmathworkscomproductsroboticshtml21httpswwwscipyorg22httpsscikit-learnorgstable
quaternion algebra It has the unique feature of using a notationvery close to the mathematical description and supportingthree different programming languages namely MATLABPython and C++ while using very similar conventions amongthem making it very easy to switch between languages andthus shortening the development time from prototyping tothe implementation on actual robots The library currentlysupports serial manipulator robots cooperative systems suchas two-arm robots mobile manipulators and even branchedmechanisms such as humanoids Robot models are generatedautomatically from simple geometrical parameters such asthe Denavit-Hartenberg parameters In addition robots canbe easily combined to yield more complex ones while thecorresponding forward kinematics and differential kinematicsare also computed automatically and analytically at executiontime The library also offers a rich set of motion controllersand it is very simple to implement new ones Although it doesnot offer dynamics modeling yet it can be easily integrated toexisting libraries for that purpose However robot dynamicsmodeling using dual quaternion algebra is currently underdevelopment and will be integrated into the library as soonas it becomes mature Rather than being a competitor toexisting libraries DQ Robotics aims at complementing themand helping in the popularization of dual quaternion algebrain the robotics domain
ACKNOWLEDGMENT
The authors would like to thank all users of the DQ Roboticslibrary for their valuable feedback and bug reports speciallyJuan Joseacute Quiroz-Omantildea and other members of the MACROresearch group at UFMG
REFERENCES
[1] O Bottema and B Roth Theoretical kinematics North-HollandPublishing Company 1979 vol 24
[2] J McCarthy Introduction to theoretical kinematics 1st ed The MITPress 1990
[3] B V Adorno ldquoTwo-arm Manipulation From Manipulators to EnhancedHuman-Robot Collaboration [Contribution agrave la manipulation agrave deuxbras des manipulateurs agrave la collaboration homme-robot]rdquo PhDDissertation Universiteacute Montpellier 2 2011 [Online] Availablehttpstelarchives-ouvertesfrtel-00641678
[4] M W Spong and M Vidyasagar Robot dynamics and control JohnWiley amp Sons 2008
[5] B Siciliano L Sciavicco L Villani and G Oriolo Robotics modellingplanning and control Springer Science amp Business Media 2010
[6] B Siciliano and O Khatib Springer handbook of robotics Springer2016
[7] G Leclercq P Lefegravevre and G Blohm ldquo3D kinematics using dualquaternions theory and applications in neurosciencerdquo Frontiers inBehavioral Neuroscience vol 7 no February p 7 jan 2013 [Online]Available httpwwwncbinlmnihgovpubmed23443667httpjournalfrontiersinorgarticle103389fnbeh201300007abstract
[8] B V Adorno ldquoRobot Kinematic Modeling and Control Based on DualQuaternion Algebra ndash Part I Fundamentalsrdquo 2017 [Online] Availablehttpshalarchives-ouvertesfrhal-01478225v1
[9] M Quigley K Conley B Gerkey J Faust T Foote J LeibsR Wheeler and A Y Ng ldquoRos an open-source robot operating systemrdquoin ICRA workshop on open source software vol 3 no 32 Kobe Japan2009 p 5
[10] E Rohmer S P N Singh and M Freese ldquoV-REP A versatile andscalable robot simulation frameworkrdquo in 2013 IEEERSJ InternationalConference on Intelligent Robots and Systems IEEE nov 2013
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 12
[11] M M Marinho B V Adorno K Harada and M Mitsuishi ldquoDynamicActive Constraints for Surgical Robots Using Vector-Field InequalitiesrdquoIEEE Transactions on Robotics vol 35 no 5 pp 1166ndash1185 oct 2019[Online] Available httpsieeexploreieeeorgdocument8742769
[12] M M Marinho K Harada A Morita and M Mitsuishi ldquoSmartarmIntegration and validation of a versatile surgical robotic system forconstrained workspacesrdquo The International Journal of Medical Roboticsand Computer Assisted Surgery (in press) 2019
[13] J J Quiroz-Omana and B V Adorno ldquoWhole-Body Control With(Self) Collision Avoidance Using Vector Field Inequalitiesrdquo IEEERobotics and Automation Letters vol 4 no 4 pp 4048ndash4053 oct 2019
[Online] Available httpsieeexploreieeeorgdocument8763977[14] H J Savino L C Pimenta J A Shah and B V Adorno
ldquoPose consensus based on dual quaternion algebra with applicationto decentralized formation control of mobile manipulatorsrdquo Journalof the Franklin Institute pp 1ndash36 oct 2019 [Online] AvailablehttpslinkinghubelseviercomretrievepiiS0016003219307161
[15] P Corke Robotics Vision and Control 2nd ed ser SpringerTracts in Advanced Robotics Springer International Publishing2017 vol 118 [Online] Available httplinkspringercom101007978-3-319-54413-7
- I Introduction
-
- I-A How to follow the tutorial
- I-B Notation choice and common operations
-
- II Representing rigid motions
- III Robot kinematics
- IV Geometric Primitives
- V Robot motion control
- VI Interface with ROS
- VII Interface with V-REP
-
- VII-A A more complete example
-
- VIII Development infrastructure
- IX Applications and comparison with other packages and libraries
- X Conclusions
- References
-
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 2
A How to follow the tutorial
In this paper for brevity we present several code snippets inMATLAB language that should be familiar to most roboticistsSuitable commands are also available in Python and C++The readers might be interested in downloading and installingthe DQ Robotics toolbox5 for MATLAB and trying out thecode snippets in their own machine More details are given inSection VIII and in the DQ Robotics documentation6
The tutorial also has a few diagrams using the UnifiedModeling Language (UML) which we use to explain theobject-oriented modeling of the library in a way that isagnostic to the programming language A simplified UMLdiagram that shows the relevant object-oriented concepts usedin this paper is briefly explained in Fig 1
Fig 1 A simplified class diagram in the Unified Modeling Language (UML)Abstract methods and classes are italicized Concrete methods and classes arein upright text Static method are underlined Specific arrowheads indicateinheritance and aggregation
B Notation choice and common operations
Given the imaginary units ık that satisfy ı2 = 2 = k2 =ık = minus1 and the dual unit ε which satisfies ε 6= 0 andε2 = 0 the quaternion set is defined as
H h1 + ıh2 + h3 + kh4 h1 h2 h3 h4 isin R
and the dual quaternion set is defined as
H h1 + εh2 h1h2 isin H
There are several equivalent notations for representing dualquaternions but we adopt the hypercomplex one which re-gards them as an extension of complex numbers This waysums multiplications and subtractions are exactly the sameused in complex numbers (and also in real numbers) differ-ently from the scalar-plus-vector notation which requires theredefinition of the multiplication operation When designingDQ Robotics one important guiding principle was to be ableto use dual quaternion operations as close as possible to theway that we do it on paper closing the gap between theoryand implementation
For instance the dual quaternions a = ı + ε(1 + k
)and
b = minus2 + + ε(ı+ k
)are declared in MATLAB as
a = DQi + DQE(1 + DQk)b = -2 + DQj + DQE(DQi + DQk)
5httpsgithubcomdqroboticsmatlabreleaseslatest6httpsdqroboticsgithubioreadthedocsioenlatestinstallationhtml
and
gtgt a + bans = ( - 2 + 1i + 1j) + E(1 + 1i + 2k)gtgt a bans = ( - 2i + 1k) + E( - 3 - 1i - 2k)
Alternatively one can use include_namespace_dq to enablethe aliases i_ j_ k_ and E_ Therefore
gtgt E_aans = E(1i)
The list of the main DQ class operations is shown in Table I
TABLE IMAIN METHODS OF CLASS DQ
Binary operations between two dual quaternions (eg a + b)+ - Sum subtraction and multiplication Right division and left division
Unary operations (eg -a and arsquo)- Minus[rsquo] [rsquo] Conjugate and sharp conjugate
Unary operators (eg P(a))conj sharp Conjugate and sharp conjugateexp log Exponential of pure dual quaternions and logarithm
of unit dual quaternionsinv Inverse under multiplicationhamiplus4haminus4
Hamilton operators of a quaternion
hamiplus8haminus8
Hamilton operators of a dual quaternion
norm Norm of dual quaternionsP D Primary part and dual partRe Im Real component and imaginary componentsrotation Rotation component of a unit dual quaternionrotation_axisrotation_angle
Rotation axis and rotation angle of a unit dualquaternion
vec3 vec6 Three-dimensional and six-dimensional vectorscomposed of the coefficients of the imaginary part ofa quaternion and a dual quaternion respectively
vec4 vec8 Four-dimensional and eight-dimensional vectorscomposed of the coefficients of a quaternion and adual quaternion respectively
Binary operators (eg Ad(ab))Ad Adsharp Adjoint operation and sharp adjointcross dot Cross product and dot product
Binary relations (eg a == b)== ~= Equal and not equal
Common methods[plot] Plots coordinate systems lines and planes
Methods and operations enclosed by brackets (eg []) are available onlyon MATLAB
II REPRESENTING RIGID MOTIONS
Since the set of dual quaternions represents an eight-dimensional space it is particularly useful to represent ro-tations translations and more generally rigid motions Forinstance the set S3 h isin H h = 1 is used to representrotations with h
radichhlowast =
radich21 + h22 + h23 + h24 being
the quaternion norm and hlowast = h1minus(h2 ı+h3+h4k) being theconjugate of h = h1 + h2 ı+ h3+ h4k Any element r isin S3can always be written as r = cos (φ2) + n sin (φ2) wheren = nx ı+ ny + nz k isin S3 is the unit-norm rotation axis andφ isin [0 2π) is the rotation angle Therefore a rotation of π radaround the x-axis is given by rπx cos(π2)+ ı sin(π2) =ı
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 3
Since the multiplication of unit quaternions is also a unitquaternion a sequence of rotations is given by a sequence ofmultiplications of unit quaternions For instance if first weperform the rotation rπx and then we use the rotated frameto do another rotation of π rad around the y-axis given byrπy cos(π2) + sin(π2) = then the final rotation isgiven by rπxrπy = ı = k because ık = minus1 and k2 = minus1Besides because k = cos(π2)+ k sin(π2) then rπxrπy =rπz which represents the rotation of π rad around the z-axisThis is illustrated in Fig 2
Fig 2 Sequence of rotations represented by multiplication of unit quater-nions Each arrow has the same color of the rotation axis
The set of pure quaternions is defined as Hp h1 ı+ h2+ h3k h1 h2 h3 isin R
sub H and is used to
represent elements of a tridimensional space For instance thepoint (x y z) can be directly mapped to xı+ y+ zk whichis an element of Hp Furthermore given a point p0 isin Hp inframe F0 and the unit quaternion r1 isin S3 that represents therotation from F0 to F1 the coordinates of the point in F1 isp1 = rlowast1p
0r1 Analogously given r2 isin S3 that represents therotation from F1 to F2 the coordinates of the point in F2
is p2 = rlowast2p1r2 = rlowast2r
lowast1p
0r1r2 which implies that the unitquaternion that represent the rotation from F0 to F2 is givenby r1r2 showing again that a sequence of rotations is givenby multiplication of unit quaternions
Translations and rotations can be grouped into a singleunit dual quaternion More specifically a rigid motion isrepresented by
x = r + ε1
2pr (1)
as illustrated in Fig 3 and a sequence of rigid motions isgiven by multiplication of unit dual quaternions For a moredetailed description of the fundamentals of dual quaternionalgebra see [8]
Using DQ Robotics a translation of (01 02 03)m fol-lowed by a rotation of π rad around the y-axis using (1) isdeclared in MATLAB as
Listing 1 Declaring a unit dual quaternion in MATLABr = cos(pi2) + j_sin(pi2)p = 01i_ + 02j_ + 03k_xd = r + E_05pr
Alternatively one could declare the rotation and translation as
Fig 3 A rigid motion x between frames Fa and Fb given by a translationp followed by a rotation r
r = DQ([cos(pi2) 0 sin(pi2) 0])p = DQ([0 01 02 03])
Whereas the first way is closer to the mathematical notationthe second one can be convenient when using the matrixfunctions available in MATLAB Python and C++
III ROBOT KINEMATICS
The implementation of robot kinematic modeling is doneby means of the abstract class DQ_Kinematics and all itssubclasses whose hierarchy is shown in Fig 4 The librarycurrently supports serial manipulators mobile bases and theircomposition which results in mobile manipulators (managedby the DQ_WholeBody class) bimanual systems (managedby the DQ_CooperativeDualTaskSpace class) and evenbranched mechanisms such as bimanual mobile manipulatorsCommon manipulator robots are available such as the KUKALWR4 and the Barrett WAM Arm but custom robots can beeasily created by using arbitrary Denavit-Hartenberg parame-ters Some common mobile robots such as the differential-drive iRobot Create are also available Creating new onesrequires only the wheels radiuses and the axis length
Let us consider the KUKA YouBot which is a mobile ma-nipulator composed of a 5-DOF manipulator serially coupledto a holonomic base One easy way of defining it on DQRobotics is to model the arm and the mobile base separatelyand then assemble them to form a mobile manipulator Theserial arm is defined with the help of the Denavit-Hartenbergparameters as followsarm_DH_theta = [0 pi2 0 pi2 0]arm_DH_d = [0147 0 0 0 0218]arm_DH_a = [0 0155 0135 0 0]arm_DH_alpha = [pi2 0 0 pi2 0]arm_DH_matrix = [arm_DH_theta arm_DH_d arm_DH_a
arm_DH_alpha]arm = DQ_SerialManipulator(arm_DH_matrix rsquostandardrsquo)
The holonomic base is defined asbase = DQ_HolonomicBase()
and then they are coupled together in a DQ_WholeBody objectx_bm = 1 + E_05(022575i_ + 01441k_)baseset_frame_displacement(x_bm)robot = DQ_WholeBody(base)robotadd(arm)
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 4
Since there is a displacement between the mobile base frameand the location where the arm is attached we use the methodset_frame_displacement() New kinematic chains can beserially coupled to the last chain by using the method add()
Because the KUKA Youbot is already defined in DQRobotics it suffices to instantiate an object of the KukaYoubotclass
youbot = KukaYoubotkinematics()
All standard robots in DQ Robotics are defined inside thefolder [root_folder]robots and have the static methodkinematics() that returns a DQ_Kinematics object There-fore a KUKA LWR 4 robot manipulator can be definedanalogously
lwr4 = KukaLwr4Robotkinematics()
All DQ_Kinematics subclasses have common functions forrobot kinematics such as the ones used to calculate the forwardkinematics the Jacobian matrix that maps the configurationvelocities to the time-derivative of the end-effector pose aswell as other Jacobian matrices that map the configurationvelocities to the time derivative of other geometrical primitivesattached to the end-effector such as lines and planes Thisincludes DQ_WholeBody objects therefore the correspondingJacobians are whole-body Jacobians that take into account thecomplete kinematic chain The list of the main methods issummarized in the simplified UML diagram in Fig 4 anddetailed in Table II
IV GEOMETRIC PRIMITIVES
Several geometrical primitives such as points planes andlines are represented as elements of the dual quaternion algebra[8] which is particularly useful when incorporating geometricconstraints into robot motion controllers For instance a planeexpressed in a frame Fa is given by πa = na + εda wherena = nx ı + ny + nz k is the normal to the plane andda = 〈pana〉 is the signed distance between the plane andthe origin of Fa and pa is an arbitrary point on the plane asillustrated in Fig 5
Therefore the plane πa = + ε15 (ie a plane whosenormal is parallel to the y-axis and whose distance from Fais 15 m) is defined in DQ Robotics as
plane_a = j_ + E_ 15
Analogously a line in Fa with direction given by la = lx ı+ly + lz k and passing through point sa = px ı+ py + pz k isrepresented in dual quaternion algebra as la = la + εsa times laas illustrated in Fig 5 Therefore a line parallel to the y-axispassing through point sa = ı (ie with coordinates (1 0 0))is defined in DQ Robotics as
line_a = j_ + E_ cross(i_j_)
In MATLAB the plot functions are available for framesplanes and lines
plot(DQ(1)rsquonamersquo rsquo$mathcalF_a$rsquo)plot(plane_a rsquoplanersquo 20 rsquocolorrsquo rsquomagentarsquo)plot(line_a rsquolinersquo 5)
Fig 4 Simplified UML class diagram of the robot modeling classes with themain methods that are available in C++ Python and MATLAB
The result is shown in Fig 6The library implements the utility class DQ_Geometry
which provides methods for geometric calculations be-tween primitives such as distances Jacobians relating therobot joint velocities to primitives attached to the end-effector are available in the DQ_Kinematics class suchas line_jacobian() and plane_jacobian() In addi-tion DQ_Kinematics contains several Jacobians that re-late the time derivative of the distance between two primi-tives (eg plane_to_point_distance_jacobian()) whichare grouped in the UML class diagram of Fig 4 asprim_dist_jacobian()
V ROBOT MOTION CONTROL
Several advantages make dual quaternion algebra attractivewhen designing controllers On the one hand unit dual quater-nions are easily mapped into a vector structure The vectorstructure is particularly convenient in pose control as there isno need to use intermediate mappings nor extract parameters
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 5
TABLE IIMAIN METHODS OF CLASS DQ_Kinematics AND ITS SUBCLASSES
DQ_Kinematicsfkm Compute the forward kinematicspose_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of the pose of a frame attached to
the robotset_base_frame Set the physical location of the robot in spaceset_reference_frame Set the reference frame for all calculationsline_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of a line attached to the robotplane_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of a plane attached to the robotrotation_jacobian Compute only the rotational part of the pose_jacobiantranslation_jacobian Compute only the translational part of the pose_jacobianprim_dist_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of the distance between a
primitive attached to the robot and another primitive in the workspace For instance prim_dist_jacobian can beline_to_line_distance_jacobian line_to_point_distance_jacobianplane_to_point_distance_jacobian point_to_line_distance_jacobianpoint_to_plane_distance_jacobian point_to_point_distance_jacobian
DQ_SerialManipulatorpose_jacobian_derivative Compute the analytical time derivative of the pose Jacobian matrixset_effector Set a constant transformation for the end-effector pose with respect to the frame attached to the end of the last link
DQ_MobileBase DQ_HolonomicBase and DQ_DifferentialDriveRobotset_frame_displacement Set the rigid transformation for the base frameset_base_diameter Change the base diameterconstraint_jacobian Compute the Jacobian that relates the wheels velocities to the configuration velocities
DQ_WholeBodyadd Add a new element to the end of the serially coupled kinematic chainadd_reversed Add a new element but in reverse order to the end of the serially coupled kinematic chainget_chain Returns the complete kinematic chainsequential Reorganize a sequential configuration vector in the ordering required by each kinematic chain (ie the vector blocks
corresponding to reversed chains are reversed)
DQ_CooperativeDualTaskSpaceabsolute_pose Compute the pose of a frame between the two end-effectors which is usually related to a grasped objectabsolute_pose_jacobian Compute the Jacobian that maps the joint velocities of the two-arm system to the time derivative of the absolute posepose1 and pose2 Compute the poses of the first and second end-effectors respectivelypose_jacobian1 andpose_jacobian2
Compute the Jacobians that maps the configurations velocities to the time derivative of the poses of the first andsecond end-effectors respectively
relative_pose Compute the rigid transformation between the two end-effectorsrelative_pose_jacobian Compute the Jacobian that maps the joint velocities of the two-arm system to the time derivative of the relative pose
Abstract methods are written in italics concrete methods are written in upright and static methods are underlined Concrete methods that implement theirabstract counterparts are omitted for the sake of conciseness
Fig 5 Two geometric primitives represented by elements of the dualquaternion algebra (a) the blue plane πa represented by the normal na
and the distance to the origin da = 〈pana〉 where pa is an arbitrary pointon the plane (b) and the green line la represented by the direction la andthe moment sa times la where sa is an arbitrary point on the line
Fig 6 MATLAB plot reference frame Fa line la = + ε(ıtimes ) parallelto the y-axis and passing through point (1 0 0) and plane πa = + ε15with normal parallel to the y-axis and whose distance from the origin of Fa
is 15 m
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 6
from the dual quaternion On the other hand the position iseasily extracted from the HTM but to obtain the orientationone usually has to extract the rotation angle and the rotationaxis from the rotation matrix This extraction introduces rep-resentational singularities when the rotation angle equals zeroFurthermore when designing constrained controllers based ongeometrical constraints one has to represent several geometri-cal primitives in different coordinate systems and in differentlocations Dual quaternion algebra is particularly useful in thiscase because several relevant geometrical primitives are easilyrepresented as dual quaternions as shown in Section IV
Users can implement their own controllers in DQ RoboticsFor instance suppose that the desired end-effector pose isgiven by Listing 1 and the initial robot configuration andcontroller parameters are given in Listing 2
Listing 2 Parameters for a simple kinematic controllerq = [0 03770 01257 -05655 0 0 0]rsquoT = 0001 sampling timegain = 10 controller gain
A classic controller based on an Euclidean error function andthe Jacobian pseudo-inverse can be implemented in MATLABas shown in Listing 3
Listing 3 Kinematic controller based on the Jacobian pseudoinversee = ones(81) initialize the error vectorwhile norm(e) gt 0001
J = lwr4pose_jacobian(q)x = lwr4fkm(q)e = vec8(x-xd)u = -pinv(J)gaineq = q + Tu
end
In Listing 3 when executing the controller on a real robot thecontrol input u is usually sent directly to the robot
There are several kinematic controllers included in DQRobotics including the one in Listing 3 The class hierarchyfor the kinematic controllers is summarized by the simplifiedUML diagram in Fig 7 All kinematic controller classes inheritfrom the abstract superclass DQ_KinematicController andtheir main methods are detailed in Table III
The DQ_KinematicController subclasses are focused onthe control structure not on particular geometrical tasks Thesame subclass can be used regardless if the goal is to controlthe end-effector pose position orientation etc In order todistinguish between task objectives it suffices to usecontrollerset_control_objective(GOAL)
where GOAL is an object from the enumeration classControlObjective which currently provides the follow-ing enumeration members Distance DistanceToPlaneLine Plane Pose Rotation and Translation Thereforethe implementation in Listing 3 can be rewritten using theDQ_PseudoinverseController class as
Listing 4 Controller based on the DQ_PseudoinverseController classcontrol = DQ_PseudoinverseController(lwr4)controlset_control_objective(ControlObjectivePose)
controlset_gain(gain)controlset_stability_threshold(00001)while ~controlsystem_reached_stable_region()
Fig 7 Simplified UML class diagram of the robot kinematic controller classeswith the main methods that are available in C++ Python and MATLAB
u = controlcompute_setpoint_control_signal(qvec8(xd))q = q + Tu
end
Furthermore if the geometric task objective changes theDQ_KinematicController subclass is responsible for calcu-lating the appropriate forward kinematics and the correspond-ing Jacobian For instance if a line is attached to the end-effector and the goal is to align it with a line in the workspacethen it suffices to replace the second line of Listing 4 bycontrolset_primitive_to_effector(line)controlset_control_objective(ControlObjectiveLine)
where line is a dual quaternion corresponding to a linepassing through the origin of the end-effector frame
Furthermore since any subclass ofDQ_KinematicController provides the methods usedin Listing 4 changing the controller is a matter of changingjust one line of code namely the first one in Listing 4
VI INTERFACE WITH ROSThe Robot Operating System (ROS) [9] is widely used
by the robotics community After the one-line installationdescribed in Section VIII the Python version of the DQRobotics library can be imported in any Python script in thesystem therefore it is compatible with ROS out-of-the-boxMATLAB also has an interface with ROS7
For C++ code in its most recent versions ROS uses thecatkin_build8 environment which itself depends on CMAKEGiven that DQ Robotics C++ is installed as a system library inUbuntu as shown in Section VIII ROS users can readily haveaccess to the library as they would to any other system librarywith a proper CMAKE configuration After installation theC++ version of the library can be linked with
7httpswwwmathworkscomhelprosugget-started-with-roshtml8httpscatkin-toolsreadthedocsioenlatestverbscatkin_buildhtml
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 7
TABLE IIIMAIN METHODS OF CLASS DQ_KinematicController AND ITS MAIN SUBCLASSES
DQ_KinematicControllercompute_setpoint_control_signal Compute the control input to regulate to a set-pointcompute_tracking_control_signal Compute the control input to track a trajectoryget_control_objective Return the control objectiveget_jacobian Return the correct Jacobian based on the control objectiveget_task_variable Return the task variable based on the control objectiveis_set Verify if the controller is set and ready to be usedset_control_objective Set the control objective using predefined goals in ControlObjectiveset_damping Set the damping to prevent instabilities near singular configurationsset_gain Set the controller gainset_primitive_to_effector Attach primitive (eg plane line point) to the end-effectorset_stability_threshold Set the threshold that determines if a stable region has been reachedsystem_reached_stable_region Return true if the trajectories of the closed-loop system have reached a stable region (ie a
positive invariant set) false otherwiseverify_stability Verify if the closed-loop region has reached a stable region
DQ_PseudoinverseSetpointControllercompute_tracking_control_signal Given the task error x = xminus xd isin Rn and the feedforward term xd compute the control
signal u = J+ (minusλx+ xd) where J+ is the pseudoinverse of the task Jacobian J and λis the controller gain
DQ_KinematicConstrainedControllerset_equality_constraint Add equality constraints of type Bq = b where q isin Rn is the vector of joint velocities
b isin Rm is the vector of equality constraints and B isin Rmtimesnset_inequality_constraint Add inequality constraints of type Aq a where q isin Rn is the vector of joint velocities
a isin Rm is the vector of inequality constraints and A isin Rmtimesn
DQ_TaskspaceQuadraticProgrammingControllercompute_objective_function_linear_component Compute the vector h used in the objective function (12)qTHq + hT qcompute_objective_function_symmetric_matrix Compute the matrix H used in the objective function (12)qTHq + hT qcompute_tracking_control_signal Given the task error x = xminus xd isin Rn compute the control signal given by
u isin argminq(12)qTHq + hT q subject to Aq a and Bq = b
DQ_ClassicQPControllercompute_objective_function_linear_component Compute the vector h used in (12)qTHq + hT q = Jq + λxminus xd22compute_objective_function_symmetric_matrix Compute the matrix H used in (12)qTHq + hT q = Jq + λxminus xd22
Abstract methods are written in italics and concrete methods are written in upright In all concrete classes the methodcompute_setpoint_control_signal() does not include the feedforward term in the method parameters and is equivalent to the methodcompute_tracking_control_signal with xd = 0 Therefore they are omitted for the sake of conciseness
target_link_libraries(my_binary dqrobotics)
for a given binary called my_binary
VII INTERFACE WITH V-REPDQ Robotics provides a simple interface to V-REP [10]
enabling users to develop complex simulations without havingto delve into the V-REP documentation The V-REP interfacecomes bundled with the MATLAB and Python versions of thelibrary The C++ version of our V-REP interface comes asa separate package and the following CMAKE directive willlink the required shared objecttarget_link_libraries(my_binary
dqroboticsdqrobotics -vrep-interface)
for a given my_binaryThe available methods are shown in Table IV
To start the communication with V-REP using its remoteAPI9 only two methods are necessaryvi = DQ_VrepInterfaceviconnect(rsquo127001rsquo19997)vistart_simulation()
9httpwwwcoppeliaroboticscomhelpFilesenlegacyRemoteApiOverviewhtm
TABLE IVVrepInterface
connect Connect to a V-REP Remote API Serverdisconnect Disconnect from currently connected serverdisconnect_all Flush all Remote API connectionsstart_simulation Start V-REP simulationstop_simulation Stop V-REP simulationget_object_translation Get object translation as a pure quaternionset_object_translation Set object translation with a pure quaternionget_object_rotation Get object rotation as a unit quaternionset_object_rotation Set object rotation with a unit quaternionget_object_pose Get object pose as a unit dual quaternionset_object_pose Set object pose with a unit dual quaternionset_joint_positions Set the joint positions of a manipulator robotset_joint_target_positions Set the joint target positions of a manipulator
robotget_joint_positions Get the joint positions of a manipulator robot
The method start_simulation starts the V-REP simulationwith the default asynchronous mode and the recommended 5ms communication thread cycle10
Since each robot joint in V-REP is associated with a nameit is convenient to encapsulate this information inside the robotclass Therefore classes implementing robots that use the V-
10For other parameters refer to the code
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 8
REP interface must be a subclass of DQ_VrepRobot to pro-vide the methods send_q_to_vrep and get_q_from_vrepwhich are used to send the desired robot configuration vectorto V-REP and get the current robot configuration vector fromV-REP respectively
Objects poses in a V-REP scene can be retrievedor set by using the methods get_object_pose andset_object_pose respectively which are useful when de-signing motion planners or controllers that take into accountthe constraints imposed by obstacles Finally in order to endthe V-REP simulation it suffices to use two methodsvistop_simulation()vidisconnect()
A A more complete example
To better highlight how DQ Robotics can be used withV-REP in this example a KUKA LWR4 manipulator robotinteracts with a KUKA YouBot mobile manipulator in aworkspace containing three obstacles namely two cylindersand a plane as shown in Fig 8
Fig 8 A more complete example The reference frame is given by F thetime-varying frame Fc is used to indicate where the circle must be drawn inthe whiteboard and Fmm is the end-effector frame of the mobile manipulatorObstacles are represented as geometrical primitives the wall is representedby the plane π = nπ + ε〈pπ nπ〉 where pπ is an arbitrary point onthe plane and nπ is the plane normal and the two cylindrical obstaclesare represented by (l1 R1) and (l2 R2) where li = li + εpi times li isthe ith cylinder centerline and Ri is the corresponding cylinder radius withi isin 1 2 Furthermore li is the line direction and pi is an arbitrary pointon the ith line To prevent collisions between the robot and the obstacles therobot is represented by a circle given by (p3 R3) where p3 and R3 are thecenter and radius of the circle respectively
The manipulator robot holds a whiteboard and uses apseudo-inverse-based controller with a feedforward term totrack a trajectory given by xm(t) = rm(t)xm(0)pm(t)where rm = cos(φ(t)2) + k sin(φ(t)2) with φ(t) =(π2) sin(ωnt) and p = 1 + (12)εdz cos(ωdt)k Viewedfrom the top the whiteboard will follow a semi-circular pathwith a radial oscillation at a frequency of ωd rads andamplitude dzm and an oscillation around the vertical axisat a frequency of ωn rads The mobile manipulator holds afelt pen and follows the manipulator trajectory while drawing a
circle on the whiteboard The mobile manipulator desired end-effector trajectory is given by xmm(t) = xm(t)xc wherexc = 1 + (12)ε0015k is a constant displacement of 15cmalong the z-axis to account for the whiteboard width and = cos(π2) + sin(π2) is a rotation of π around they-axis so that both end-effectors have their z-axis pointingto opposite directions To prevent collision with obstaclesit uses a constrained controller in which for each obstaclewe use a differential inequality to ensure that the robot willapproach it in the worst case with an exponential velocitydecrease For each obstacle we define a distance metricd (t) d (t)minus dsafe where dsafe isin [0infin) is an arbitraryconstant safe distance and the following inequalities must holdfor all t
˙d (t) ge minusηdd (t)lArrrArr minusJdq le ηdd (t) (2)
where ηd isin [0infin) is used to adjust the approach velocity andJd is the distance Jacobian related to the obstacle [11] Thelower is ηd the lower is the allowed approach velocity
In the case of our example to prevent collisions betweenthe robot and the wall we let dsafe R3 (see Fig 8) suchthat d (t) = 0 lArrrArr d (t) = R3 and the distance function isdefined as the distance between p3 which is the center of thecircle enclosing the robot and the wall plane π Analogouslyto prevent collisions between the robot and the static cylinderswe use one inequality such as (2) for each cylinder wheredsafe Ri + R3 and d(t) is defined as the distance betweenp3 and the lines li for i isin 1 2 Although those distancefunctions and corresponding Jacobians can be computed byusing the classes DQ_Geometry and DQ_Kinematics respec-tively details of how they are obtained using dual quaternionalgebra are presented in [11]
Whenever the felt-tip is sufficiently close to the whiteboardit starts to draw a circle Figs 9a and 9b show that the mobilemanipulator successfully draws the circle although there aresome small imperfections as the obstacle avoidance is a hardconstraint that is always respected at the expense of worsetrajectory tracking Last Fig 9c shows the mobile manipulatortrajectory which is always adapted to prevent collisions withthe cylinders and the wall
The main MATLAB code is shown in Listing 5 For the sakeof conciseness the functions compute_lwr4_referenceand compute_youbot_reference which calculate theaforementioned end-effector trajectories are omittedThe same applies for get_plane_from_vrep andget_line_from_vrep which get information about thelocation of the corresponding geometric primitives inV-REP and compute_constraints which calculatesconstraints such as (2) for each obstacle in the scene Theoriginal source code used in this example is available assupplementary material For a more up-to-date version referto httpsgithubcomdqroboticsmatlab-examples
VIII DEVELOPMENT INFRASTRUCTURE
Aiming at the scalability of the DQ Robotics library andtaking advantage of the familiarity that current developers havewith Git and Github we opted to also use those services
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 9
(a) Circle partially drawn at t = 36s
(b) Circle completely drawn at t = 170s
(c) Top view showing the mobile manipulator trajectory
Fig 9 Manipulator robot interacting with a mobile manipulator in aworkspace with two cylindrical obstacles and a wall The parameters for thetrajectory generation were ωn = 01 ωd = 05 and dz = 01
End-users are agnostic to these infrastructural decisions Forthem usage performance and installation of the library matterthe most The usage of the library has been simplified byrelying on a programming syntax similar to the mathemati-
Listing 5 Main MATLAB code for the simulationsampling_time = 005total_time = 200for t=0sampling_timetotal_time
Get obstacles from V-REPplane = get_plane_from_vrep(virsquoObstaclePlanersquoDQk)cylinder1 = get_line_from_vrep(virsquoObstacleCylinder1rsquoDQk)cylinder2 = get_line_from_vrep(virsquoObstacleCylinder2rsquoDQk)
Set references for both robots[lwr4_xd lwr4_ff] = compute_lwr4_reference(lwr4
simulation_parameters lwr4_x0 t)[youbot_xd youbot_ff] = compute_youbot_reference(
youbot_control lwr4_xd lwr4_ff)
Compute the control input for the manipulatorlwr4_u = lwr4_controllercompute_tracking_control_signal(
lwr4_q vec8(lwr4_xd)vec8(lwr4_ff))
Compute constrained control input for the youbot[Jconstraint bconstraint] = compute_constraints(youbot
youbot_q planecylinder1 cylinder2)youbot_controlset_inequality_constraint(-Jconstraint
1bconstraint)youbot_u= youbot_controlcompute_tracking_control_signal(
youbot_q vec8(youbot_xd) vec8(youbot_ff))
Since we are using V-REP just for visualization integrate the control signal to update the robots configurationslwr4_q = lwr4_q + sampling_timelwr4_uyoubot_q = youbot_q + sampling_timeyoubot_u
Send desired values to V-REPlwr4_vreprobotsend_q_to_vrep(lwr4_q)youbot_vreprobotsend_q_to_vrep(youbot_q)end
cal description and being attentive to proper object-orientedprogramming practices Furthermore good performance isachieved by careful optimization of the library and by theC++ implementation
The ease of installation has been addressed in language-platform specific ways that we document in details on the DQRoboticsrsquo Read the Docs11
For MATLAB DQ Robotics is distributed as a Toolboxthat can be installed by the end-user in any of the MATLAB-compatible operating systems
For C++ we officially focus on Ubuntu long-term service(LTS) distributions in a similar way to the most recentdistributions of ROS and other large open-source libraries suchas TensorFlow12 For any Ubuntu LTS distribution that has notreached its end-of-life13 the user can install the C++ versionof the library with the following three commandssudo add-apt-repository ppadqrobotics -devreleasesudo apt-get updatesudo apt-get install libdqrobotics
made available via our release Personal Package Archive(PPA)14 in which we store the latest stable versions of thelibrary Interfaces between DQ Robotics and other librariesalso reside in the same PPA For example after addingthe PPA the user can install the V-REP interface with thefollowing commandsudo apt-get install libdqrobotics -interface -vrep
11httpsdqroboticsgithubioreadthedocsioenlatestinstallationhtml12httpswwwtensorfloworg13httpsubuntucomaboutrelease-cycle14httpslaunchpadnet~dqrobotics-dev+archiveubunturelease
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 10
The PPA guarantees that the user can reliably install thepackage in their system as the PPA only stores packages thatcompiled successfully Support for other operating systemswill be driven by user interest but should not be a bigchallenge for most systems since we are using only CMAKEand Eigen3 as project dependencies both of which are widelysupported
Lastly the Python version of DQ Robotics is made avail-able through a Pybind11-based15 Python wrapper of the C++library This has three main advantages First there is no needto redevelop and maintain versions of the libraries in differentprogramming languages which is highly demanding Secondunit-testing code written in Python which is easier to writeand maintain automatically validates the C++ library as wellThird we can have Pythonrsquos ease-of-use with C++rsquos perfor-mance for each function Although there is a computationaloverhead when using the Python bindings (in contrast withdirectly using the C++ code) it is much lower than a nativePython implementation A simple example of this can be seenin Table V in which we compare the average required timefor dual quaternion multiplications in the same machine
TABLE VCOMPUTATIONAL TIME OF DUAL-QUATERNION MULTIPLICATIONS
Mean [micros] Standard Deviation [micros]C++ (gcc 540) 022 00149
MATLAB R2019a 894 01223Python 352 Bindings 082 00242Python 352 Native 3142 01646
Mean and standard deviation of the required time for one dual-quaternion multiplicationThis was calculated from a thousand sets of a thousand executions on the same IntelCore i9 9900K Ubuntu 1604 x64 system and excludes the time required to generate therandom dual quaternions
An Ubuntu LTS user can install the Python3 version of DQRobotics from the Python Package Index (PyPI)16 with a singleline of code
python3 -m pip install --user dqrobotics
Lastly the PyPI package is continuously built from sourceusing TravisCI17 so that the code is properly compiled andtested before being distributed to our users
IX APPLICATIONS AND COMPARISON WITH OTHERPACKAGES AND LIBRARIES
DQ Robotics has been used for more than nine years tomodel and control different robots in applications such asbimanual surgical robots in constrained workspaces (Fig 10a)whole-body control of humanoid robots (Fig 10b) andthe decentralized formation control of mobile manipulators(Fig 10c)18
The main difference from other packages and librariesis that DQ Robotics implements dual quaternion algebra in
15httpsgithubcompybindpybind1116httpspypiorg17httpstravis-cicom18A more complete list of papers and applications can be found in https
dqroboticsgithubiocitationshtml
(a) The bimanual surgical robot SmartArm [12]
(b) Whole-body control of a humanoid robot [13]
(c) Cooperative mobile manipulators [14]
Fig 10 Examples of different applications of DQ Robotics
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 11
a close way to the mathematical notation Furthermore allsupported languages namely Python MATLAB and C++use the same convention and style and it is straightforwardto port code in one language to other supported languages Toenable that we avoid using features that are not available in allthree languages unless that would lead to poor-quality codeFrom the user point-of-view the advantage of such approachis that users can prototype their ideas on MATLAB usingthe convenient plot system then easily translate the code toC++ and deploy it on a real robotic platform The Pythonversion of the library is somewhere between MATLAB andC++ being suitable for prototyping and also for deploymenton real applications because C++ runs under the hood
In terms of target audience the MATLAB version of DQRobotics has some overlap with Peter Corkersquos Robotics Tool-box [15] most notably in terms of their use for educationand learning On the one hand earlier versions were greatlyinspired by Peter Corkersquos Robotics Toolbox most notablythe plot system which is particularly useful when teachingrobotics On the other hand DQ Robotics is entirely basedon general dual quaternion algebra whereas Robotics Tool-box uses classic representations for robot modeling althoughit does offer support for unit quaternions Moreover otherfunctionalities of Peter Corkersquos Robotics Toolbox such aspath planning localization and mapping are not available inDQ Robotics as of now However some functionalities ofPeter Corkersquos Robotics Toolbox are now available in toolboxesdistributed by MathWorks such as the Image ProcessingToolbox19 and the Robotics System Toolbox20 which are inprinciple compatible with DQ Robotics
Another MATLAB toolbox [7] focuses on the application ofunit dual quaternions to the neuroscience domain and presentsbasic kinematic modeling of serial mechanisms In addition tohaving many fewer functionalities than DQ Robotics it usesprocedural programming which makes the code quite differentfrom the notation used on paper
A major contrast between DQ Robotics and Peter Corkersquosand Leclercqrsquos MATLAB toolboxes is that DQ Robotics hasPython and C++ versions with identical APIs as far as theprogramming languages permit This increases the numberof possible users and considerably reduces the time requiredto go from prototyping to deployment Moreover this makesDQ Robotics compatible with powerful libraries available inPython such as SciPy21 and scikit-learn22 In addition bothPython and C++ versions of the library can be readily usedalongside ROS which is the current standard of shareablecode in the robotics community Lastly a modern developmentinfrastructure not available in other existing libraries makesthe library scalable and easier to maintain and extend
X CONCLUSIONS
This paper presented DQ Robotics a comprehensive com-putational library for robot modeling and control using dual
19httpswwwmathworkscomproductsimagehtml20httpswwwmathworkscomproductsroboticshtml21httpswwwscipyorg22httpsscikit-learnorgstable
quaternion algebra It has the unique feature of using a notationvery close to the mathematical description and supportingthree different programming languages namely MATLABPython and C++ while using very similar conventions amongthem making it very easy to switch between languages andthus shortening the development time from prototyping tothe implementation on actual robots The library currentlysupports serial manipulator robots cooperative systems suchas two-arm robots mobile manipulators and even branchedmechanisms such as humanoids Robot models are generatedautomatically from simple geometrical parameters such asthe Denavit-Hartenberg parameters In addition robots canbe easily combined to yield more complex ones while thecorresponding forward kinematics and differential kinematicsare also computed automatically and analytically at executiontime The library also offers a rich set of motion controllersand it is very simple to implement new ones Although it doesnot offer dynamics modeling yet it can be easily integrated toexisting libraries for that purpose However robot dynamicsmodeling using dual quaternion algebra is currently underdevelopment and will be integrated into the library as soonas it becomes mature Rather than being a competitor toexisting libraries DQ Robotics aims at complementing themand helping in the popularization of dual quaternion algebrain the robotics domain
ACKNOWLEDGMENT
The authors would like to thank all users of the DQ Roboticslibrary for their valuable feedback and bug reports speciallyJuan Joseacute Quiroz-Omantildea and other members of the MACROresearch group at UFMG
REFERENCES
[1] O Bottema and B Roth Theoretical kinematics North-HollandPublishing Company 1979 vol 24
[2] J McCarthy Introduction to theoretical kinematics 1st ed The MITPress 1990
[3] B V Adorno ldquoTwo-arm Manipulation From Manipulators to EnhancedHuman-Robot Collaboration [Contribution agrave la manipulation agrave deuxbras des manipulateurs agrave la collaboration homme-robot]rdquo PhDDissertation Universiteacute Montpellier 2 2011 [Online] Availablehttpstelarchives-ouvertesfrtel-00641678
[4] M W Spong and M Vidyasagar Robot dynamics and control JohnWiley amp Sons 2008
[5] B Siciliano L Sciavicco L Villani and G Oriolo Robotics modellingplanning and control Springer Science amp Business Media 2010
[6] B Siciliano and O Khatib Springer handbook of robotics Springer2016
[7] G Leclercq P Lefegravevre and G Blohm ldquo3D kinematics using dualquaternions theory and applications in neurosciencerdquo Frontiers inBehavioral Neuroscience vol 7 no February p 7 jan 2013 [Online]Available httpwwwncbinlmnihgovpubmed23443667httpjournalfrontiersinorgarticle103389fnbeh201300007abstract
[8] B V Adorno ldquoRobot Kinematic Modeling and Control Based on DualQuaternion Algebra ndash Part I Fundamentalsrdquo 2017 [Online] Availablehttpshalarchives-ouvertesfrhal-01478225v1
[9] M Quigley K Conley B Gerkey J Faust T Foote J LeibsR Wheeler and A Y Ng ldquoRos an open-source robot operating systemrdquoin ICRA workshop on open source software vol 3 no 32 Kobe Japan2009 p 5
[10] E Rohmer S P N Singh and M Freese ldquoV-REP A versatile andscalable robot simulation frameworkrdquo in 2013 IEEERSJ InternationalConference on Intelligent Robots and Systems IEEE nov 2013
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 12
[11] M M Marinho B V Adorno K Harada and M Mitsuishi ldquoDynamicActive Constraints for Surgical Robots Using Vector-Field InequalitiesrdquoIEEE Transactions on Robotics vol 35 no 5 pp 1166ndash1185 oct 2019[Online] Available httpsieeexploreieeeorgdocument8742769
[12] M M Marinho K Harada A Morita and M Mitsuishi ldquoSmartarmIntegration and validation of a versatile surgical robotic system forconstrained workspacesrdquo The International Journal of Medical Roboticsand Computer Assisted Surgery (in press) 2019
[13] J J Quiroz-Omana and B V Adorno ldquoWhole-Body Control With(Self) Collision Avoidance Using Vector Field Inequalitiesrdquo IEEERobotics and Automation Letters vol 4 no 4 pp 4048ndash4053 oct 2019
[Online] Available httpsieeexploreieeeorgdocument8763977[14] H J Savino L C Pimenta J A Shah and B V Adorno
ldquoPose consensus based on dual quaternion algebra with applicationto decentralized formation control of mobile manipulatorsrdquo Journalof the Franklin Institute pp 1ndash36 oct 2019 [Online] AvailablehttpslinkinghubelseviercomretrievepiiS0016003219307161
[15] P Corke Robotics Vision and Control 2nd ed ser SpringerTracts in Advanced Robotics Springer International Publishing2017 vol 118 [Online] Available httplinkspringercom101007978-3-319-54413-7
- I Introduction
-
- I-A How to follow the tutorial
- I-B Notation choice and common operations
-
- II Representing rigid motions
- III Robot kinematics
- IV Geometric Primitives
- V Robot motion control
- VI Interface with ROS
- VII Interface with V-REP
-
- VII-A A more complete example
-
- VIII Development infrastructure
- IX Applications and comparison with other packages and libraries
- X Conclusions
- References
-
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 3
Since the multiplication of unit quaternions is also a unitquaternion a sequence of rotations is given by a sequence ofmultiplications of unit quaternions For instance if first weperform the rotation rπx and then we use the rotated frameto do another rotation of π rad around the y-axis given byrπy cos(π2) + sin(π2) = then the final rotation isgiven by rπxrπy = ı = k because ık = minus1 and k2 = minus1Besides because k = cos(π2)+ k sin(π2) then rπxrπy =rπz which represents the rotation of π rad around the z-axisThis is illustrated in Fig 2
Fig 2 Sequence of rotations represented by multiplication of unit quater-nions Each arrow has the same color of the rotation axis
The set of pure quaternions is defined as Hp h1 ı+ h2+ h3k h1 h2 h3 isin R
sub H and is used to
represent elements of a tridimensional space For instance thepoint (x y z) can be directly mapped to xı+ y+ zk whichis an element of Hp Furthermore given a point p0 isin Hp inframe F0 and the unit quaternion r1 isin S3 that represents therotation from F0 to F1 the coordinates of the point in F1 isp1 = rlowast1p
0r1 Analogously given r2 isin S3 that represents therotation from F1 to F2 the coordinates of the point in F2
is p2 = rlowast2p1r2 = rlowast2r
lowast1p
0r1r2 which implies that the unitquaternion that represent the rotation from F0 to F2 is givenby r1r2 showing again that a sequence of rotations is givenby multiplication of unit quaternions
Translations and rotations can be grouped into a singleunit dual quaternion More specifically a rigid motion isrepresented by
x = r + ε1
2pr (1)
as illustrated in Fig 3 and a sequence of rigid motions isgiven by multiplication of unit dual quaternions For a moredetailed description of the fundamentals of dual quaternionalgebra see [8]
Using DQ Robotics a translation of (01 02 03)m fol-lowed by a rotation of π rad around the y-axis using (1) isdeclared in MATLAB as
Listing 1 Declaring a unit dual quaternion in MATLABr = cos(pi2) + j_sin(pi2)p = 01i_ + 02j_ + 03k_xd = r + E_05pr
Alternatively one could declare the rotation and translation as
Fig 3 A rigid motion x between frames Fa and Fb given by a translationp followed by a rotation r
r = DQ([cos(pi2) 0 sin(pi2) 0])p = DQ([0 01 02 03])
Whereas the first way is closer to the mathematical notationthe second one can be convenient when using the matrixfunctions available in MATLAB Python and C++
III ROBOT KINEMATICS
The implementation of robot kinematic modeling is doneby means of the abstract class DQ_Kinematics and all itssubclasses whose hierarchy is shown in Fig 4 The librarycurrently supports serial manipulators mobile bases and theircomposition which results in mobile manipulators (managedby the DQ_WholeBody class) bimanual systems (managedby the DQ_CooperativeDualTaskSpace class) and evenbranched mechanisms such as bimanual mobile manipulatorsCommon manipulator robots are available such as the KUKALWR4 and the Barrett WAM Arm but custom robots can beeasily created by using arbitrary Denavit-Hartenberg parame-ters Some common mobile robots such as the differential-drive iRobot Create are also available Creating new onesrequires only the wheels radiuses and the axis length
Let us consider the KUKA YouBot which is a mobile ma-nipulator composed of a 5-DOF manipulator serially coupledto a holonomic base One easy way of defining it on DQRobotics is to model the arm and the mobile base separatelyand then assemble them to form a mobile manipulator Theserial arm is defined with the help of the Denavit-Hartenbergparameters as followsarm_DH_theta = [0 pi2 0 pi2 0]arm_DH_d = [0147 0 0 0 0218]arm_DH_a = [0 0155 0135 0 0]arm_DH_alpha = [pi2 0 0 pi2 0]arm_DH_matrix = [arm_DH_theta arm_DH_d arm_DH_a
arm_DH_alpha]arm = DQ_SerialManipulator(arm_DH_matrix rsquostandardrsquo)
The holonomic base is defined asbase = DQ_HolonomicBase()
and then they are coupled together in a DQ_WholeBody objectx_bm = 1 + E_05(022575i_ + 01441k_)baseset_frame_displacement(x_bm)robot = DQ_WholeBody(base)robotadd(arm)
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 4
Since there is a displacement between the mobile base frameand the location where the arm is attached we use the methodset_frame_displacement() New kinematic chains can beserially coupled to the last chain by using the method add()
Because the KUKA Youbot is already defined in DQRobotics it suffices to instantiate an object of the KukaYoubotclass
youbot = KukaYoubotkinematics()
All standard robots in DQ Robotics are defined inside thefolder [root_folder]robots and have the static methodkinematics() that returns a DQ_Kinematics object There-fore a KUKA LWR 4 robot manipulator can be definedanalogously
lwr4 = KukaLwr4Robotkinematics()
All DQ_Kinematics subclasses have common functions forrobot kinematics such as the ones used to calculate the forwardkinematics the Jacobian matrix that maps the configurationvelocities to the time-derivative of the end-effector pose aswell as other Jacobian matrices that map the configurationvelocities to the time derivative of other geometrical primitivesattached to the end-effector such as lines and planes Thisincludes DQ_WholeBody objects therefore the correspondingJacobians are whole-body Jacobians that take into account thecomplete kinematic chain The list of the main methods issummarized in the simplified UML diagram in Fig 4 anddetailed in Table II
IV GEOMETRIC PRIMITIVES
Several geometrical primitives such as points planes andlines are represented as elements of the dual quaternion algebra[8] which is particularly useful when incorporating geometricconstraints into robot motion controllers For instance a planeexpressed in a frame Fa is given by πa = na + εda wherena = nx ı + ny + nz k is the normal to the plane andda = 〈pana〉 is the signed distance between the plane andthe origin of Fa and pa is an arbitrary point on the plane asillustrated in Fig 5
Therefore the plane πa = + ε15 (ie a plane whosenormal is parallel to the y-axis and whose distance from Fais 15 m) is defined in DQ Robotics as
plane_a = j_ + E_ 15
Analogously a line in Fa with direction given by la = lx ı+ly + lz k and passing through point sa = px ı+ py + pz k isrepresented in dual quaternion algebra as la = la + εsa times laas illustrated in Fig 5 Therefore a line parallel to the y-axispassing through point sa = ı (ie with coordinates (1 0 0))is defined in DQ Robotics as
line_a = j_ + E_ cross(i_j_)
In MATLAB the plot functions are available for framesplanes and lines
plot(DQ(1)rsquonamersquo rsquo$mathcalF_a$rsquo)plot(plane_a rsquoplanersquo 20 rsquocolorrsquo rsquomagentarsquo)plot(line_a rsquolinersquo 5)
Fig 4 Simplified UML class diagram of the robot modeling classes with themain methods that are available in C++ Python and MATLAB
The result is shown in Fig 6The library implements the utility class DQ_Geometry
which provides methods for geometric calculations be-tween primitives such as distances Jacobians relating therobot joint velocities to primitives attached to the end-effector are available in the DQ_Kinematics class suchas line_jacobian() and plane_jacobian() In addi-tion DQ_Kinematics contains several Jacobians that re-late the time derivative of the distance between two primi-tives (eg plane_to_point_distance_jacobian()) whichare grouped in the UML class diagram of Fig 4 asprim_dist_jacobian()
V ROBOT MOTION CONTROL
Several advantages make dual quaternion algebra attractivewhen designing controllers On the one hand unit dual quater-nions are easily mapped into a vector structure The vectorstructure is particularly convenient in pose control as there isno need to use intermediate mappings nor extract parameters
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 5
TABLE IIMAIN METHODS OF CLASS DQ_Kinematics AND ITS SUBCLASSES
DQ_Kinematicsfkm Compute the forward kinematicspose_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of the pose of a frame attached to
the robotset_base_frame Set the physical location of the robot in spaceset_reference_frame Set the reference frame for all calculationsline_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of a line attached to the robotplane_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of a plane attached to the robotrotation_jacobian Compute only the rotational part of the pose_jacobiantranslation_jacobian Compute only the translational part of the pose_jacobianprim_dist_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of the distance between a
primitive attached to the robot and another primitive in the workspace For instance prim_dist_jacobian can beline_to_line_distance_jacobian line_to_point_distance_jacobianplane_to_point_distance_jacobian point_to_line_distance_jacobianpoint_to_plane_distance_jacobian point_to_point_distance_jacobian
DQ_SerialManipulatorpose_jacobian_derivative Compute the analytical time derivative of the pose Jacobian matrixset_effector Set a constant transformation for the end-effector pose with respect to the frame attached to the end of the last link
DQ_MobileBase DQ_HolonomicBase and DQ_DifferentialDriveRobotset_frame_displacement Set the rigid transformation for the base frameset_base_diameter Change the base diameterconstraint_jacobian Compute the Jacobian that relates the wheels velocities to the configuration velocities
DQ_WholeBodyadd Add a new element to the end of the serially coupled kinematic chainadd_reversed Add a new element but in reverse order to the end of the serially coupled kinematic chainget_chain Returns the complete kinematic chainsequential Reorganize a sequential configuration vector in the ordering required by each kinematic chain (ie the vector blocks
corresponding to reversed chains are reversed)
DQ_CooperativeDualTaskSpaceabsolute_pose Compute the pose of a frame between the two end-effectors which is usually related to a grasped objectabsolute_pose_jacobian Compute the Jacobian that maps the joint velocities of the two-arm system to the time derivative of the absolute posepose1 and pose2 Compute the poses of the first and second end-effectors respectivelypose_jacobian1 andpose_jacobian2
Compute the Jacobians that maps the configurations velocities to the time derivative of the poses of the first andsecond end-effectors respectively
relative_pose Compute the rigid transformation between the two end-effectorsrelative_pose_jacobian Compute the Jacobian that maps the joint velocities of the two-arm system to the time derivative of the relative pose
Abstract methods are written in italics concrete methods are written in upright and static methods are underlined Concrete methods that implement theirabstract counterparts are omitted for the sake of conciseness
Fig 5 Two geometric primitives represented by elements of the dualquaternion algebra (a) the blue plane πa represented by the normal na
and the distance to the origin da = 〈pana〉 where pa is an arbitrary pointon the plane (b) and the green line la represented by the direction la andthe moment sa times la where sa is an arbitrary point on the line
Fig 6 MATLAB plot reference frame Fa line la = + ε(ıtimes ) parallelto the y-axis and passing through point (1 0 0) and plane πa = + ε15with normal parallel to the y-axis and whose distance from the origin of Fa
is 15 m
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 6
from the dual quaternion On the other hand the position iseasily extracted from the HTM but to obtain the orientationone usually has to extract the rotation angle and the rotationaxis from the rotation matrix This extraction introduces rep-resentational singularities when the rotation angle equals zeroFurthermore when designing constrained controllers based ongeometrical constraints one has to represent several geometri-cal primitives in different coordinate systems and in differentlocations Dual quaternion algebra is particularly useful in thiscase because several relevant geometrical primitives are easilyrepresented as dual quaternions as shown in Section IV
Users can implement their own controllers in DQ RoboticsFor instance suppose that the desired end-effector pose isgiven by Listing 1 and the initial robot configuration andcontroller parameters are given in Listing 2
Listing 2 Parameters for a simple kinematic controllerq = [0 03770 01257 -05655 0 0 0]rsquoT = 0001 sampling timegain = 10 controller gain
A classic controller based on an Euclidean error function andthe Jacobian pseudo-inverse can be implemented in MATLABas shown in Listing 3
Listing 3 Kinematic controller based on the Jacobian pseudoinversee = ones(81) initialize the error vectorwhile norm(e) gt 0001
J = lwr4pose_jacobian(q)x = lwr4fkm(q)e = vec8(x-xd)u = -pinv(J)gaineq = q + Tu
end
In Listing 3 when executing the controller on a real robot thecontrol input u is usually sent directly to the robot
There are several kinematic controllers included in DQRobotics including the one in Listing 3 The class hierarchyfor the kinematic controllers is summarized by the simplifiedUML diagram in Fig 7 All kinematic controller classes inheritfrom the abstract superclass DQ_KinematicController andtheir main methods are detailed in Table III
The DQ_KinematicController subclasses are focused onthe control structure not on particular geometrical tasks Thesame subclass can be used regardless if the goal is to controlthe end-effector pose position orientation etc In order todistinguish between task objectives it suffices to usecontrollerset_control_objective(GOAL)
where GOAL is an object from the enumeration classControlObjective which currently provides the follow-ing enumeration members Distance DistanceToPlaneLine Plane Pose Rotation and Translation Thereforethe implementation in Listing 3 can be rewritten using theDQ_PseudoinverseController class as
Listing 4 Controller based on the DQ_PseudoinverseController classcontrol = DQ_PseudoinverseController(lwr4)controlset_control_objective(ControlObjectivePose)
controlset_gain(gain)controlset_stability_threshold(00001)while ~controlsystem_reached_stable_region()
Fig 7 Simplified UML class diagram of the robot kinematic controller classeswith the main methods that are available in C++ Python and MATLAB
u = controlcompute_setpoint_control_signal(qvec8(xd))q = q + Tu
end
Furthermore if the geometric task objective changes theDQ_KinematicController subclass is responsible for calcu-lating the appropriate forward kinematics and the correspond-ing Jacobian For instance if a line is attached to the end-effector and the goal is to align it with a line in the workspacethen it suffices to replace the second line of Listing 4 bycontrolset_primitive_to_effector(line)controlset_control_objective(ControlObjectiveLine)
where line is a dual quaternion corresponding to a linepassing through the origin of the end-effector frame
Furthermore since any subclass ofDQ_KinematicController provides the methods usedin Listing 4 changing the controller is a matter of changingjust one line of code namely the first one in Listing 4
VI INTERFACE WITH ROSThe Robot Operating System (ROS) [9] is widely used
by the robotics community After the one-line installationdescribed in Section VIII the Python version of the DQRobotics library can be imported in any Python script in thesystem therefore it is compatible with ROS out-of-the-boxMATLAB also has an interface with ROS7
For C++ code in its most recent versions ROS uses thecatkin_build8 environment which itself depends on CMAKEGiven that DQ Robotics C++ is installed as a system library inUbuntu as shown in Section VIII ROS users can readily haveaccess to the library as they would to any other system librarywith a proper CMAKE configuration After installation theC++ version of the library can be linked with
7httpswwwmathworkscomhelprosugget-started-with-roshtml8httpscatkin-toolsreadthedocsioenlatestverbscatkin_buildhtml
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 7
TABLE IIIMAIN METHODS OF CLASS DQ_KinematicController AND ITS MAIN SUBCLASSES
DQ_KinematicControllercompute_setpoint_control_signal Compute the control input to regulate to a set-pointcompute_tracking_control_signal Compute the control input to track a trajectoryget_control_objective Return the control objectiveget_jacobian Return the correct Jacobian based on the control objectiveget_task_variable Return the task variable based on the control objectiveis_set Verify if the controller is set and ready to be usedset_control_objective Set the control objective using predefined goals in ControlObjectiveset_damping Set the damping to prevent instabilities near singular configurationsset_gain Set the controller gainset_primitive_to_effector Attach primitive (eg plane line point) to the end-effectorset_stability_threshold Set the threshold that determines if a stable region has been reachedsystem_reached_stable_region Return true if the trajectories of the closed-loop system have reached a stable region (ie a
positive invariant set) false otherwiseverify_stability Verify if the closed-loop region has reached a stable region
DQ_PseudoinverseSetpointControllercompute_tracking_control_signal Given the task error x = xminus xd isin Rn and the feedforward term xd compute the control
signal u = J+ (minusλx+ xd) where J+ is the pseudoinverse of the task Jacobian J and λis the controller gain
DQ_KinematicConstrainedControllerset_equality_constraint Add equality constraints of type Bq = b where q isin Rn is the vector of joint velocities
b isin Rm is the vector of equality constraints and B isin Rmtimesnset_inequality_constraint Add inequality constraints of type Aq a where q isin Rn is the vector of joint velocities
a isin Rm is the vector of inequality constraints and A isin Rmtimesn
DQ_TaskspaceQuadraticProgrammingControllercompute_objective_function_linear_component Compute the vector h used in the objective function (12)qTHq + hT qcompute_objective_function_symmetric_matrix Compute the matrix H used in the objective function (12)qTHq + hT qcompute_tracking_control_signal Given the task error x = xminus xd isin Rn compute the control signal given by
u isin argminq(12)qTHq + hT q subject to Aq a and Bq = b
DQ_ClassicQPControllercompute_objective_function_linear_component Compute the vector h used in (12)qTHq + hT q = Jq + λxminus xd22compute_objective_function_symmetric_matrix Compute the matrix H used in (12)qTHq + hT q = Jq + λxminus xd22
Abstract methods are written in italics and concrete methods are written in upright In all concrete classes the methodcompute_setpoint_control_signal() does not include the feedforward term in the method parameters and is equivalent to the methodcompute_tracking_control_signal with xd = 0 Therefore they are omitted for the sake of conciseness
target_link_libraries(my_binary dqrobotics)
for a given binary called my_binary
VII INTERFACE WITH V-REPDQ Robotics provides a simple interface to V-REP [10]
enabling users to develop complex simulations without havingto delve into the V-REP documentation The V-REP interfacecomes bundled with the MATLAB and Python versions of thelibrary The C++ version of our V-REP interface comes asa separate package and the following CMAKE directive willlink the required shared objecttarget_link_libraries(my_binary
dqroboticsdqrobotics -vrep-interface)
for a given my_binaryThe available methods are shown in Table IV
To start the communication with V-REP using its remoteAPI9 only two methods are necessaryvi = DQ_VrepInterfaceviconnect(rsquo127001rsquo19997)vistart_simulation()
9httpwwwcoppeliaroboticscomhelpFilesenlegacyRemoteApiOverviewhtm
TABLE IVVrepInterface
connect Connect to a V-REP Remote API Serverdisconnect Disconnect from currently connected serverdisconnect_all Flush all Remote API connectionsstart_simulation Start V-REP simulationstop_simulation Stop V-REP simulationget_object_translation Get object translation as a pure quaternionset_object_translation Set object translation with a pure quaternionget_object_rotation Get object rotation as a unit quaternionset_object_rotation Set object rotation with a unit quaternionget_object_pose Get object pose as a unit dual quaternionset_object_pose Set object pose with a unit dual quaternionset_joint_positions Set the joint positions of a manipulator robotset_joint_target_positions Set the joint target positions of a manipulator
robotget_joint_positions Get the joint positions of a manipulator robot
The method start_simulation starts the V-REP simulationwith the default asynchronous mode and the recommended 5ms communication thread cycle10
Since each robot joint in V-REP is associated with a nameit is convenient to encapsulate this information inside the robotclass Therefore classes implementing robots that use the V-
10For other parameters refer to the code
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 8
REP interface must be a subclass of DQ_VrepRobot to pro-vide the methods send_q_to_vrep and get_q_from_vrepwhich are used to send the desired robot configuration vectorto V-REP and get the current robot configuration vector fromV-REP respectively
Objects poses in a V-REP scene can be retrievedor set by using the methods get_object_pose andset_object_pose respectively which are useful when de-signing motion planners or controllers that take into accountthe constraints imposed by obstacles Finally in order to endthe V-REP simulation it suffices to use two methodsvistop_simulation()vidisconnect()
A A more complete example
To better highlight how DQ Robotics can be used withV-REP in this example a KUKA LWR4 manipulator robotinteracts with a KUKA YouBot mobile manipulator in aworkspace containing three obstacles namely two cylindersand a plane as shown in Fig 8
Fig 8 A more complete example The reference frame is given by F thetime-varying frame Fc is used to indicate where the circle must be drawn inthe whiteboard and Fmm is the end-effector frame of the mobile manipulatorObstacles are represented as geometrical primitives the wall is representedby the plane π = nπ + ε〈pπ nπ〉 where pπ is an arbitrary point onthe plane and nπ is the plane normal and the two cylindrical obstaclesare represented by (l1 R1) and (l2 R2) where li = li + εpi times li isthe ith cylinder centerline and Ri is the corresponding cylinder radius withi isin 1 2 Furthermore li is the line direction and pi is an arbitrary pointon the ith line To prevent collisions between the robot and the obstacles therobot is represented by a circle given by (p3 R3) where p3 and R3 are thecenter and radius of the circle respectively
The manipulator robot holds a whiteboard and uses apseudo-inverse-based controller with a feedforward term totrack a trajectory given by xm(t) = rm(t)xm(0)pm(t)where rm = cos(φ(t)2) + k sin(φ(t)2) with φ(t) =(π2) sin(ωnt) and p = 1 + (12)εdz cos(ωdt)k Viewedfrom the top the whiteboard will follow a semi-circular pathwith a radial oscillation at a frequency of ωd rads andamplitude dzm and an oscillation around the vertical axisat a frequency of ωn rads The mobile manipulator holds afelt pen and follows the manipulator trajectory while drawing a
circle on the whiteboard The mobile manipulator desired end-effector trajectory is given by xmm(t) = xm(t)xc wherexc = 1 + (12)ε0015k is a constant displacement of 15cmalong the z-axis to account for the whiteboard width and = cos(π2) + sin(π2) is a rotation of π around they-axis so that both end-effectors have their z-axis pointingto opposite directions To prevent collision with obstaclesit uses a constrained controller in which for each obstaclewe use a differential inequality to ensure that the robot willapproach it in the worst case with an exponential velocitydecrease For each obstacle we define a distance metricd (t) d (t)minus dsafe where dsafe isin [0infin) is an arbitraryconstant safe distance and the following inequalities must holdfor all t
˙d (t) ge minusηdd (t)lArrrArr minusJdq le ηdd (t) (2)
where ηd isin [0infin) is used to adjust the approach velocity andJd is the distance Jacobian related to the obstacle [11] Thelower is ηd the lower is the allowed approach velocity
In the case of our example to prevent collisions betweenthe robot and the wall we let dsafe R3 (see Fig 8) suchthat d (t) = 0 lArrrArr d (t) = R3 and the distance function isdefined as the distance between p3 which is the center of thecircle enclosing the robot and the wall plane π Analogouslyto prevent collisions between the robot and the static cylinderswe use one inequality such as (2) for each cylinder wheredsafe Ri + R3 and d(t) is defined as the distance betweenp3 and the lines li for i isin 1 2 Although those distancefunctions and corresponding Jacobians can be computed byusing the classes DQ_Geometry and DQ_Kinematics respec-tively details of how they are obtained using dual quaternionalgebra are presented in [11]
Whenever the felt-tip is sufficiently close to the whiteboardit starts to draw a circle Figs 9a and 9b show that the mobilemanipulator successfully draws the circle although there aresome small imperfections as the obstacle avoidance is a hardconstraint that is always respected at the expense of worsetrajectory tracking Last Fig 9c shows the mobile manipulatortrajectory which is always adapted to prevent collisions withthe cylinders and the wall
The main MATLAB code is shown in Listing 5 For the sakeof conciseness the functions compute_lwr4_referenceand compute_youbot_reference which calculate theaforementioned end-effector trajectories are omittedThe same applies for get_plane_from_vrep andget_line_from_vrep which get information about thelocation of the corresponding geometric primitives inV-REP and compute_constraints which calculatesconstraints such as (2) for each obstacle in the scene Theoriginal source code used in this example is available assupplementary material For a more up-to-date version referto httpsgithubcomdqroboticsmatlab-examples
VIII DEVELOPMENT INFRASTRUCTURE
Aiming at the scalability of the DQ Robotics library andtaking advantage of the familiarity that current developers havewith Git and Github we opted to also use those services
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 9
(a) Circle partially drawn at t = 36s
(b) Circle completely drawn at t = 170s
(c) Top view showing the mobile manipulator trajectory
Fig 9 Manipulator robot interacting with a mobile manipulator in aworkspace with two cylindrical obstacles and a wall The parameters for thetrajectory generation were ωn = 01 ωd = 05 and dz = 01
End-users are agnostic to these infrastructural decisions Forthem usage performance and installation of the library matterthe most The usage of the library has been simplified byrelying on a programming syntax similar to the mathemati-
Listing 5 Main MATLAB code for the simulationsampling_time = 005total_time = 200for t=0sampling_timetotal_time
Get obstacles from V-REPplane = get_plane_from_vrep(virsquoObstaclePlanersquoDQk)cylinder1 = get_line_from_vrep(virsquoObstacleCylinder1rsquoDQk)cylinder2 = get_line_from_vrep(virsquoObstacleCylinder2rsquoDQk)
Set references for both robots[lwr4_xd lwr4_ff] = compute_lwr4_reference(lwr4
simulation_parameters lwr4_x0 t)[youbot_xd youbot_ff] = compute_youbot_reference(
youbot_control lwr4_xd lwr4_ff)
Compute the control input for the manipulatorlwr4_u = lwr4_controllercompute_tracking_control_signal(
lwr4_q vec8(lwr4_xd)vec8(lwr4_ff))
Compute constrained control input for the youbot[Jconstraint bconstraint] = compute_constraints(youbot
youbot_q planecylinder1 cylinder2)youbot_controlset_inequality_constraint(-Jconstraint
1bconstraint)youbot_u= youbot_controlcompute_tracking_control_signal(
youbot_q vec8(youbot_xd) vec8(youbot_ff))
Since we are using V-REP just for visualization integrate the control signal to update the robots configurationslwr4_q = lwr4_q + sampling_timelwr4_uyoubot_q = youbot_q + sampling_timeyoubot_u
Send desired values to V-REPlwr4_vreprobotsend_q_to_vrep(lwr4_q)youbot_vreprobotsend_q_to_vrep(youbot_q)end
cal description and being attentive to proper object-orientedprogramming practices Furthermore good performance isachieved by careful optimization of the library and by theC++ implementation
The ease of installation has been addressed in language-platform specific ways that we document in details on the DQRoboticsrsquo Read the Docs11
For MATLAB DQ Robotics is distributed as a Toolboxthat can be installed by the end-user in any of the MATLAB-compatible operating systems
For C++ we officially focus on Ubuntu long-term service(LTS) distributions in a similar way to the most recentdistributions of ROS and other large open-source libraries suchas TensorFlow12 For any Ubuntu LTS distribution that has notreached its end-of-life13 the user can install the C++ versionof the library with the following three commandssudo add-apt-repository ppadqrobotics -devreleasesudo apt-get updatesudo apt-get install libdqrobotics
made available via our release Personal Package Archive(PPA)14 in which we store the latest stable versions of thelibrary Interfaces between DQ Robotics and other librariesalso reside in the same PPA For example after addingthe PPA the user can install the V-REP interface with thefollowing commandsudo apt-get install libdqrobotics -interface -vrep
11httpsdqroboticsgithubioreadthedocsioenlatestinstallationhtml12httpswwwtensorfloworg13httpsubuntucomaboutrelease-cycle14httpslaunchpadnet~dqrobotics-dev+archiveubunturelease
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 10
The PPA guarantees that the user can reliably install thepackage in their system as the PPA only stores packages thatcompiled successfully Support for other operating systemswill be driven by user interest but should not be a bigchallenge for most systems since we are using only CMAKEand Eigen3 as project dependencies both of which are widelysupported
Lastly the Python version of DQ Robotics is made avail-able through a Pybind11-based15 Python wrapper of the C++library This has three main advantages First there is no needto redevelop and maintain versions of the libraries in differentprogramming languages which is highly demanding Secondunit-testing code written in Python which is easier to writeand maintain automatically validates the C++ library as wellThird we can have Pythonrsquos ease-of-use with C++rsquos perfor-mance for each function Although there is a computationaloverhead when using the Python bindings (in contrast withdirectly using the C++ code) it is much lower than a nativePython implementation A simple example of this can be seenin Table V in which we compare the average required timefor dual quaternion multiplications in the same machine
TABLE VCOMPUTATIONAL TIME OF DUAL-QUATERNION MULTIPLICATIONS
Mean [micros] Standard Deviation [micros]C++ (gcc 540) 022 00149
MATLAB R2019a 894 01223Python 352 Bindings 082 00242Python 352 Native 3142 01646
Mean and standard deviation of the required time for one dual-quaternion multiplicationThis was calculated from a thousand sets of a thousand executions on the same IntelCore i9 9900K Ubuntu 1604 x64 system and excludes the time required to generate therandom dual quaternions
An Ubuntu LTS user can install the Python3 version of DQRobotics from the Python Package Index (PyPI)16 with a singleline of code
python3 -m pip install --user dqrobotics
Lastly the PyPI package is continuously built from sourceusing TravisCI17 so that the code is properly compiled andtested before being distributed to our users
IX APPLICATIONS AND COMPARISON WITH OTHERPACKAGES AND LIBRARIES
DQ Robotics has been used for more than nine years tomodel and control different robots in applications such asbimanual surgical robots in constrained workspaces (Fig 10a)whole-body control of humanoid robots (Fig 10b) andthe decentralized formation control of mobile manipulators(Fig 10c)18
The main difference from other packages and librariesis that DQ Robotics implements dual quaternion algebra in
15httpsgithubcompybindpybind1116httpspypiorg17httpstravis-cicom18A more complete list of papers and applications can be found in https
dqroboticsgithubiocitationshtml
(a) The bimanual surgical robot SmartArm [12]
(b) Whole-body control of a humanoid robot [13]
(c) Cooperative mobile manipulators [14]
Fig 10 Examples of different applications of DQ Robotics
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 11
a close way to the mathematical notation Furthermore allsupported languages namely Python MATLAB and C++use the same convention and style and it is straightforwardto port code in one language to other supported languages Toenable that we avoid using features that are not available in allthree languages unless that would lead to poor-quality codeFrom the user point-of-view the advantage of such approachis that users can prototype their ideas on MATLAB usingthe convenient plot system then easily translate the code toC++ and deploy it on a real robotic platform The Pythonversion of the library is somewhere between MATLAB andC++ being suitable for prototyping and also for deploymenton real applications because C++ runs under the hood
In terms of target audience the MATLAB version of DQRobotics has some overlap with Peter Corkersquos Robotics Tool-box [15] most notably in terms of their use for educationand learning On the one hand earlier versions were greatlyinspired by Peter Corkersquos Robotics Toolbox most notablythe plot system which is particularly useful when teachingrobotics On the other hand DQ Robotics is entirely basedon general dual quaternion algebra whereas Robotics Tool-box uses classic representations for robot modeling althoughit does offer support for unit quaternions Moreover otherfunctionalities of Peter Corkersquos Robotics Toolbox such aspath planning localization and mapping are not available inDQ Robotics as of now However some functionalities ofPeter Corkersquos Robotics Toolbox are now available in toolboxesdistributed by MathWorks such as the Image ProcessingToolbox19 and the Robotics System Toolbox20 which are inprinciple compatible with DQ Robotics
Another MATLAB toolbox [7] focuses on the application ofunit dual quaternions to the neuroscience domain and presentsbasic kinematic modeling of serial mechanisms In addition tohaving many fewer functionalities than DQ Robotics it usesprocedural programming which makes the code quite differentfrom the notation used on paper
A major contrast between DQ Robotics and Peter Corkersquosand Leclercqrsquos MATLAB toolboxes is that DQ Robotics hasPython and C++ versions with identical APIs as far as theprogramming languages permit This increases the numberof possible users and considerably reduces the time requiredto go from prototyping to deployment Moreover this makesDQ Robotics compatible with powerful libraries available inPython such as SciPy21 and scikit-learn22 In addition bothPython and C++ versions of the library can be readily usedalongside ROS which is the current standard of shareablecode in the robotics community Lastly a modern developmentinfrastructure not available in other existing libraries makesthe library scalable and easier to maintain and extend
X CONCLUSIONS
This paper presented DQ Robotics a comprehensive com-putational library for robot modeling and control using dual
19httpswwwmathworkscomproductsimagehtml20httpswwwmathworkscomproductsroboticshtml21httpswwwscipyorg22httpsscikit-learnorgstable
quaternion algebra It has the unique feature of using a notationvery close to the mathematical description and supportingthree different programming languages namely MATLABPython and C++ while using very similar conventions amongthem making it very easy to switch between languages andthus shortening the development time from prototyping tothe implementation on actual robots The library currentlysupports serial manipulator robots cooperative systems suchas two-arm robots mobile manipulators and even branchedmechanisms such as humanoids Robot models are generatedautomatically from simple geometrical parameters such asthe Denavit-Hartenberg parameters In addition robots canbe easily combined to yield more complex ones while thecorresponding forward kinematics and differential kinematicsare also computed automatically and analytically at executiontime The library also offers a rich set of motion controllersand it is very simple to implement new ones Although it doesnot offer dynamics modeling yet it can be easily integrated toexisting libraries for that purpose However robot dynamicsmodeling using dual quaternion algebra is currently underdevelopment and will be integrated into the library as soonas it becomes mature Rather than being a competitor toexisting libraries DQ Robotics aims at complementing themand helping in the popularization of dual quaternion algebrain the robotics domain
ACKNOWLEDGMENT
The authors would like to thank all users of the DQ Roboticslibrary for their valuable feedback and bug reports speciallyJuan Joseacute Quiroz-Omantildea and other members of the MACROresearch group at UFMG
REFERENCES
[1] O Bottema and B Roth Theoretical kinematics North-HollandPublishing Company 1979 vol 24
[2] J McCarthy Introduction to theoretical kinematics 1st ed The MITPress 1990
[3] B V Adorno ldquoTwo-arm Manipulation From Manipulators to EnhancedHuman-Robot Collaboration [Contribution agrave la manipulation agrave deuxbras des manipulateurs agrave la collaboration homme-robot]rdquo PhDDissertation Universiteacute Montpellier 2 2011 [Online] Availablehttpstelarchives-ouvertesfrtel-00641678
[4] M W Spong and M Vidyasagar Robot dynamics and control JohnWiley amp Sons 2008
[5] B Siciliano L Sciavicco L Villani and G Oriolo Robotics modellingplanning and control Springer Science amp Business Media 2010
[6] B Siciliano and O Khatib Springer handbook of robotics Springer2016
[7] G Leclercq P Lefegravevre and G Blohm ldquo3D kinematics using dualquaternions theory and applications in neurosciencerdquo Frontiers inBehavioral Neuroscience vol 7 no February p 7 jan 2013 [Online]Available httpwwwncbinlmnihgovpubmed23443667httpjournalfrontiersinorgarticle103389fnbeh201300007abstract
[8] B V Adorno ldquoRobot Kinematic Modeling and Control Based on DualQuaternion Algebra ndash Part I Fundamentalsrdquo 2017 [Online] Availablehttpshalarchives-ouvertesfrhal-01478225v1
[9] M Quigley K Conley B Gerkey J Faust T Foote J LeibsR Wheeler and A Y Ng ldquoRos an open-source robot operating systemrdquoin ICRA workshop on open source software vol 3 no 32 Kobe Japan2009 p 5
[10] E Rohmer S P N Singh and M Freese ldquoV-REP A versatile andscalable robot simulation frameworkrdquo in 2013 IEEERSJ InternationalConference on Intelligent Robots and Systems IEEE nov 2013
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 12
[11] M M Marinho B V Adorno K Harada and M Mitsuishi ldquoDynamicActive Constraints for Surgical Robots Using Vector-Field InequalitiesrdquoIEEE Transactions on Robotics vol 35 no 5 pp 1166ndash1185 oct 2019[Online] Available httpsieeexploreieeeorgdocument8742769
[12] M M Marinho K Harada A Morita and M Mitsuishi ldquoSmartarmIntegration and validation of a versatile surgical robotic system forconstrained workspacesrdquo The International Journal of Medical Roboticsand Computer Assisted Surgery (in press) 2019
[13] J J Quiroz-Omana and B V Adorno ldquoWhole-Body Control With(Self) Collision Avoidance Using Vector Field Inequalitiesrdquo IEEERobotics and Automation Letters vol 4 no 4 pp 4048ndash4053 oct 2019
[Online] Available httpsieeexploreieeeorgdocument8763977[14] H J Savino L C Pimenta J A Shah and B V Adorno
ldquoPose consensus based on dual quaternion algebra with applicationto decentralized formation control of mobile manipulatorsrdquo Journalof the Franklin Institute pp 1ndash36 oct 2019 [Online] AvailablehttpslinkinghubelseviercomretrievepiiS0016003219307161
[15] P Corke Robotics Vision and Control 2nd ed ser SpringerTracts in Advanced Robotics Springer International Publishing2017 vol 118 [Online] Available httplinkspringercom101007978-3-319-54413-7
- I Introduction
-
- I-A How to follow the tutorial
- I-B Notation choice and common operations
-
- II Representing rigid motions
- III Robot kinematics
- IV Geometric Primitives
- V Robot motion control
- VI Interface with ROS
- VII Interface with V-REP
-
- VII-A A more complete example
-
- VIII Development infrastructure
- IX Applications and comparison with other packages and libraries
- X Conclusions
- References
-
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 4
Since there is a displacement between the mobile base frameand the location where the arm is attached we use the methodset_frame_displacement() New kinematic chains can beserially coupled to the last chain by using the method add()
Because the KUKA Youbot is already defined in DQRobotics it suffices to instantiate an object of the KukaYoubotclass
youbot = KukaYoubotkinematics()
All standard robots in DQ Robotics are defined inside thefolder [root_folder]robots and have the static methodkinematics() that returns a DQ_Kinematics object There-fore a KUKA LWR 4 robot manipulator can be definedanalogously
lwr4 = KukaLwr4Robotkinematics()
All DQ_Kinematics subclasses have common functions forrobot kinematics such as the ones used to calculate the forwardkinematics the Jacobian matrix that maps the configurationvelocities to the time-derivative of the end-effector pose aswell as other Jacobian matrices that map the configurationvelocities to the time derivative of other geometrical primitivesattached to the end-effector such as lines and planes Thisincludes DQ_WholeBody objects therefore the correspondingJacobians are whole-body Jacobians that take into account thecomplete kinematic chain The list of the main methods issummarized in the simplified UML diagram in Fig 4 anddetailed in Table II
IV GEOMETRIC PRIMITIVES
Several geometrical primitives such as points planes andlines are represented as elements of the dual quaternion algebra[8] which is particularly useful when incorporating geometricconstraints into robot motion controllers For instance a planeexpressed in a frame Fa is given by πa = na + εda wherena = nx ı + ny + nz k is the normal to the plane andda = 〈pana〉 is the signed distance between the plane andthe origin of Fa and pa is an arbitrary point on the plane asillustrated in Fig 5
Therefore the plane πa = + ε15 (ie a plane whosenormal is parallel to the y-axis and whose distance from Fais 15 m) is defined in DQ Robotics as
plane_a = j_ + E_ 15
Analogously a line in Fa with direction given by la = lx ı+ly + lz k and passing through point sa = px ı+ py + pz k isrepresented in dual quaternion algebra as la = la + εsa times laas illustrated in Fig 5 Therefore a line parallel to the y-axispassing through point sa = ı (ie with coordinates (1 0 0))is defined in DQ Robotics as
line_a = j_ + E_ cross(i_j_)
In MATLAB the plot functions are available for framesplanes and lines
plot(DQ(1)rsquonamersquo rsquo$mathcalF_a$rsquo)plot(plane_a rsquoplanersquo 20 rsquocolorrsquo rsquomagentarsquo)plot(line_a rsquolinersquo 5)
Fig 4 Simplified UML class diagram of the robot modeling classes with themain methods that are available in C++ Python and MATLAB
The result is shown in Fig 6The library implements the utility class DQ_Geometry
which provides methods for geometric calculations be-tween primitives such as distances Jacobians relating therobot joint velocities to primitives attached to the end-effector are available in the DQ_Kinematics class suchas line_jacobian() and plane_jacobian() In addi-tion DQ_Kinematics contains several Jacobians that re-late the time derivative of the distance between two primi-tives (eg plane_to_point_distance_jacobian()) whichare grouped in the UML class diagram of Fig 4 asprim_dist_jacobian()
V ROBOT MOTION CONTROL
Several advantages make dual quaternion algebra attractivewhen designing controllers On the one hand unit dual quater-nions are easily mapped into a vector structure The vectorstructure is particularly convenient in pose control as there isno need to use intermediate mappings nor extract parameters
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 5
TABLE IIMAIN METHODS OF CLASS DQ_Kinematics AND ITS SUBCLASSES
DQ_Kinematicsfkm Compute the forward kinematicspose_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of the pose of a frame attached to
the robotset_base_frame Set the physical location of the robot in spaceset_reference_frame Set the reference frame for all calculationsline_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of a line attached to the robotplane_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of a plane attached to the robotrotation_jacobian Compute only the rotational part of the pose_jacobiantranslation_jacobian Compute only the translational part of the pose_jacobianprim_dist_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of the distance between a
primitive attached to the robot and another primitive in the workspace For instance prim_dist_jacobian can beline_to_line_distance_jacobian line_to_point_distance_jacobianplane_to_point_distance_jacobian point_to_line_distance_jacobianpoint_to_plane_distance_jacobian point_to_point_distance_jacobian
DQ_SerialManipulatorpose_jacobian_derivative Compute the analytical time derivative of the pose Jacobian matrixset_effector Set a constant transformation for the end-effector pose with respect to the frame attached to the end of the last link
DQ_MobileBase DQ_HolonomicBase and DQ_DifferentialDriveRobotset_frame_displacement Set the rigid transformation for the base frameset_base_diameter Change the base diameterconstraint_jacobian Compute the Jacobian that relates the wheels velocities to the configuration velocities
DQ_WholeBodyadd Add a new element to the end of the serially coupled kinematic chainadd_reversed Add a new element but in reverse order to the end of the serially coupled kinematic chainget_chain Returns the complete kinematic chainsequential Reorganize a sequential configuration vector in the ordering required by each kinematic chain (ie the vector blocks
corresponding to reversed chains are reversed)
DQ_CooperativeDualTaskSpaceabsolute_pose Compute the pose of a frame between the two end-effectors which is usually related to a grasped objectabsolute_pose_jacobian Compute the Jacobian that maps the joint velocities of the two-arm system to the time derivative of the absolute posepose1 and pose2 Compute the poses of the first and second end-effectors respectivelypose_jacobian1 andpose_jacobian2
Compute the Jacobians that maps the configurations velocities to the time derivative of the poses of the first andsecond end-effectors respectively
relative_pose Compute the rigid transformation between the two end-effectorsrelative_pose_jacobian Compute the Jacobian that maps the joint velocities of the two-arm system to the time derivative of the relative pose
Abstract methods are written in italics concrete methods are written in upright and static methods are underlined Concrete methods that implement theirabstract counterparts are omitted for the sake of conciseness
Fig 5 Two geometric primitives represented by elements of the dualquaternion algebra (a) the blue plane πa represented by the normal na
and the distance to the origin da = 〈pana〉 where pa is an arbitrary pointon the plane (b) and the green line la represented by the direction la andthe moment sa times la where sa is an arbitrary point on the line
Fig 6 MATLAB plot reference frame Fa line la = + ε(ıtimes ) parallelto the y-axis and passing through point (1 0 0) and plane πa = + ε15with normal parallel to the y-axis and whose distance from the origin of Fa
is 15 m
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 6
from the dual quaternion On the other hand the position iseasily extracted from the HTM but to obtain the orientationone usually has to extract the rotation angle and the rotationaxis from the rotation matrix This extraction introduces rep-resentational singularities when the rotation angle equals zeroFurthermore when designing constrained controllers based ongeometrical constraints one has to represent several geometri-cal primitives in different coordinate systems and in differentlocations Dual quaternion algebra is particularly useful in thiscase because several relevant geometrical primitives are easilyrepresented as dual quaternions as shown in Section IV
Users can implement their own controllers in DQ RoboticsFor instance suppose that the desired end-effector pose isgiven by Listing 1 and the initial robot configuration andcontroller parameters are given in Listing 2
Listing 2 Parameters for a simple kinematic controllerq = [0 03770 01257 -05655 0 0 0]rsquoT = 0001 sampling timegain = 10 controller gain
A classic controller based on an Euclidean error function andthe Jacobian pseudo-inverse can be implemented in MATLABas shown in Listing 3
Listing 3 Kinematic controller based on the Jacobian pseudoinversee = ones(81) initialize the error vectorwhile norm(e) gt 0001
J = lwr4pose_jacobian(q)x = lwr4fkm(q)e = vec8(x-xd)u = -pinv(J)gaineq = q + Tu
end
In Listing 3 when executing the controller on a real robot thecontrol input u is usually sent directly to the robot
There are several kinematic controllers included in DQRobotics including the one in Listing 3 The class hierarchyfor the kinematic controllers is summarized by the simplifiedUML diagram in Fig 7 All kinematic controller classes inheritfrom the abstract superclass DQ_KinematicController andtheir main methods are detailed in Table III
The DQ_KinematicController subclasses are focused onthe control structure not on particular geometrical tasks Thesame subclass can be used regardless if the goal is to controlthe end-effector pose position orientation etc In order todistinguish between task objectives it suffices to usecontrollerset_control_objective(GOAL)
where GOAL is an object from the enumeration classControlObjective which currently provides the follow-ing enumeration members Distance DistanceToPlaneLine Plane Pose Rotation and Translation Thereforethe implementation in Listing 3 can be rewritten using theDQ_PseudoinverseController class as
Listing 4 Controller based on the DQ_PseudoinverseController classcontrol = DQ_PseudoinverseController(lwr4)controlset_control_objective(ControlObjectivePose)
controlset_gain(gain)controlset_stability_threshold(00001)while ~controlsystem_reached_stable_region()
Fig 7 Simplified UML class diagram of the robot kinematic controller classeswith the main methods that are available in C++ Python and MATLAB
u = controlcompute_setpoint_control_signal(qvec8(xd))q = q + Tu
end
Furthermore if the geometric task objective changes theDQ_KinematicController subclass is responsible for calcu-lating the appropriate forward kinematics and the correspond-ing Jacobian For instance if a line is attached to the end-effector and the goal is to align it with a line in the workspacethen it suffices to replace the second line of Listing 4 bycontrolset_primitive_to_effector(line)controlset_control_objective(ControlObjectiveLine)
where line is a dual quaternion corresponding to a linepassing through the origin of the end-effector frame
Furthermore since any subclass ofDQ_KinematicController provides the methods usedin Listing 4 changing the controller is a matter of changingjust one line of code namely the first one in Listing 4
VI INTERFACE WITH ROSThe Robot Operating System (ROS) [9] is widely used
by the robotics community After the one-line installationdescribed in Section VIII the Python version of the DQRobotics library can be imported in any Python script in thesystem therefore it is compatible with ROS out-of-the-boxMATLAB also has an interface with ROS7
For C++ code in its most recent versions ROS uses thecatkin_build8 environment which itself depends on CMAKEGiven that DQ Robotics C++ is installed as a system library inUbuntu as shown in Section VIII ROS users can readily haveaccess to the library as they would to any other system librarywith a proper CMAKE configuration After installation theC++ version of the library can be linked with
7httpswwwmathworkscomhelprosugget-started-with-roshtml8httpscatkin-toolsreadthedocsioenlatestverbscatkin_buildhtml
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 7
TABLE IIIMAIN METHODS OF CLASS DQ_KinematicController AND ITS MAIN SUBCLASSES
DQ_KinematicControllercompute_setpoint_control_signal Compute the control input to regulate to a set-pointcompute_tracking_control_signal Compute the control input to track a trajectoryget_control_objective Return the control objectiveget_jacobian Return the correct Jacobian based on the control objectiveget_task_variable Return the task variable based on the control objectiveis_set Verify if the controller is set and ready to be usedset_control_objective Set the control objective using predefined goals in ControlObjectiveset_damping Set the damping to prevent instabilities near singular configurationsset_gain Set the controller gainset_primitive_to_effector Attach primitive (eg plane line point) to the end-effectorset_stability_threshold Set the threshold that determines if a stable region has been reachedsystem_reached_stable_region Return true if the trajectories of the closed-loop system have reached a stable region (ie a
positive invariant set) false otherwiseverify_stability Verify if the closed-loop region has reached a stable region
DQ_PseudoinverseSetpointControllercompute_tracking_control_signal Given the task error x = xminus xd isin Rn and the feedforward term xd compute the control
signal u = J+ (minusλx+ xd) where J+ is the pseudoinverse of the task Jacobian J and λis the controller gain
DQ_KinematicConstrainedControllerset_equality_constraint Add equality constraints of type Bq = b where q isin Rn is the vector of joint velocities
b isin Rm is the vector of equality constraints and B isin Rmtimesnset_inequality_constraint Add inequality constraints of type Aq a where q isin Rn is the vector of joint velocities
a isin Rm is the vector of inequality constraints and A isin Rmtimesn
DQ_TaskspaceQuadraticProgrammingControllercompute_objective_function_linear_component Compute the vector h used in the objective function (12)qTHq + hT qcompute_objective_function_symmetric_matrix Compute the matrix H used in the objective function (12)qTHq + hT qcompute_tracking_control_signal Given the task error x = xminus xd isin Rn compute the control signal given by
u isin argminq(12)qTHq + hT q subject to Aq a and Bq = b
DQ_ClassicQPControllercompute_objective_function_linear_component Compute the vector h used in (12)qTHq + hT q = Jq + λxminus xd22compute_objective_function_symmetric_matrix Compute the matrix H used in (12)qTHq + hT q = Jq + λxminus xd22
Abstract methods are written in italics and concrete methods are written in upright In all concrete classes the methodcompute_setpoint_control_signal() does not include the feedforward term in the method parameters and is equivalent to the methodcompute_tracking_control_signal with xd = 0 Therefore they are omitted for the sake of conciseness
target_link_libraries(my_binary dqrobotics)
for a given binary called my_binary
VII INTERFACE WITH V-REPDQ Robotics provides a simple interface to V-REP [10]
enabling users to develop complex simulations without havingto delve into the V-REP documentation The V-REP interfacecomes bundled with the MATLAB and Python versions of thelibrary The C++ version of our V-REP interface comes asa separate package and the following CMAKE directive willlink the required shared objecttarget_link_libraries(my_binary
dqroboticsdqrobotics -vrep-interface)
for a given my_binaryThe available methods are shown in Table IV
To start the communication with V-REP using its remoteAPI9 only two methods are necessaryvi = DQ_VrepInterfaceviconnect(rsquo127001rsquo19997)vistart_simulation()
9httpwwwcoppeliaroboticscomhelpFilesenlegacyRemoteApiOverviewhtm
TABLE IVVrepInterface
connect Connect to a V-REP Remote API Serverdisconnect Disconnect from currently connected serverdisconnect_all Flush all Remote API connectionsstart_simulation Start V-REP simulationstop_simulation Stop V-REP simulationget_object_translation Get object translation as a pure quaternionset_object_translation Set object translation with a pure quaternionget_object_rotation Get object rotation as a unit quaternionset_object_rotation Set object rotation with a unit quaternionget_object_pose Get object pose as a unit dual quaternionset_object_pose Set object pose with a unit dual quaternionset_joint_positions Set the joint positions of a manipulator robotset_joint_target_positions Set the joint target positions of a manipulator
robotget_joint_positions Get the joint positions of a manipulator robot
The method start_simulation starts the V-REP simulationwith the default asynchronous mode and the recommended 5ms communication thread cycle10
Since each robot joint in V-REP is associated with a nameit is convenient to encapsulate this information inside the robotclass Therefore classes implementing robots that use the V-
10For other parameters refer to the code
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 8
REP interface must be a subclass of DQ_VrepRobot to pro-vide the methods send_q_to_vrep and get_q_from_vrepwhich are used to send the desired robot configuration vectorto V-REP and get the current robot configuration vector fromV-REP respectively
Objects poses in a V-REP scene can be retrievedor set by using the methods get_object_pose andset_object_pose respectively which are useful when de-signing motion planners or controllers that take into accountthe constraints imposed by obstacles Finally in order to endthe V-REP simulation it suffices to use two methodsvistop_simulation()vidisconnect()
A A more complete example
To better highlight how DQ Robotics can be used withV-REP in this example a KUKA LWR4 manipulator robotinteracts with a KUKA YouBot mobile manipulator in aworkspace containing three obstacles namely two cylindersand a plane as shown in Fig 8
Fig 8 A more complete example The reference frame is given by F thetime-varying frame Fc is used to indicate where the circle must be drawn inthe whiteboard and Fmm is the end-effector frame of the mobile manipulatorObstacles are represented as geometrical primitives the wall is representedby the plane π = nπ + ε〈pπ nπ〉 where pπ is an arbitrary point onthe plane and nπ is the plane normal and the two cylindrical obstaclesare represented by (l1 R1) and (l2 R2) where li = li + εpi times li isthe ith cylinder centerline and Ri is the corresponding cylinder radius withi isin 1 2 Furthermore li is the line direction and pi is an arbitrary pointon the ith line To prevent collisions between the robot and the obstacles therobot is represented by a circle given by (p3 R3) where p3 and R3 are thecenter and radius of the circle respectively
The manipulator robot holds a whiteboard and uses apseudo-inverse-based controller with a feedforward term totrack a trajectory given by xm(t) = rm(t)xm(0)pm(t)where rm = cos(φ(t)2) + k sin(φ(t)2) with φ(t) =(π2) sin(ωnt) and p = 1 + (12)εdz cos(ωdt)k Viewedfrom the top the whiteboard will follow a semi-circular pathwith a radial oscillation at a frequency of ωd rads andamplitude dzm and an oscillation around the vertical axisat a frequency of ωn rads The mobile manipulator holds afelt pen and follows the manipulator trajectory while drawing a
circle on the whiteboard The mobile manipulator desired end-effector trajectory is given by xmm(t) = xm(t)xc wherexc = 1 + (12)ε0015k is a constant displacement of 15cmalong the z-axis to account for the whiteboard width and = cos(π2) + sin(π2) is a rotation of π around they-axis so that both end-effectors have their z-axis pointingto opposite directions To prevent collision with obstaclesit uses a constrained controller in which for each obstaclewe use a differential inequality to ensure that the robot willapproach it in the worst case with an exponential velocitydecrease For each obstacle we define a distance metricd (t) d (t)minus dsafe where dsafe isin [0infin) is an arbitraryconstant safe distance and the following inequalities must holdfor all t
˙d (t) ge minusηdd (t)lArrrArr minusJdq le ηdd (t) (2)
where ηd isin [0infin) is used to adjust the approach velocity andJd is the distance Jacobian related to the obstacle [11] Thelower is ηd the lower is the allowed approach velocity
In the case of our example to prevent collisions betweenthe robot and the wall we let dsafe R3 (see Fig 8) suchthat d (t) = 0 lArrrArr d (t) = R3 and the distance function isdefined as the distance between p3 which is the center of thecircle enclosing the robot and the wall plane π Analogouslyto prevent collisions between the robot and the static cylinderswe use one inequality such as (2) for each cylinder wheredsafe Ri + R3 and d(t) is defined as the distance betweenp3 and the lines li for i isin 1 2 Although those distancefunctions and corresponding Jacobians can be computed byusing the classes DQ_Geometry and DQ_Kinematics respec-tively details of how they are obtained using dual quaternionalgebra are presented in [11]
Whenever the felt-tip is sufficiently close to the whiteboardit starts to draw a circle Figs 9a and 9b show that the mobilemanipulator successfully draws the circle although there aresome small imperfections as the obstacle avoidance is a hardconstraint that is always respected at the expense of worsetrajectory tracking Last Fig 9c shows the mobile manipulatortrajectory which is always adapted to prevent collisions withthe cylinders and the wall
The main MATLAB code is shown in Listing 5 For the sakeof conciseness the functions compute_lwr4_referenceand compute_youbot_reference which calculate theaforementioned end-effector trajectories are omittedThe same applies for get_plane_from_vrep andget_line_from_vrep which get information about thelocation of the corresponding geometric primitives inV-REP and compute_constraints which calculatesconstraints such as (2) for each obstacle in the scene Theoriginal source code used in this example is available assupplementary material For a more up-to-date version referto httpsgithubcomdqroboticsmatlab-examples
VIII DEVELOPMENT INFRASTRUCTURE
Aiming at the scalability of the DQ Robotics library andtaking advantage of the familiarity that current developers havewith Git and Github we opted to also use those services
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 9
(a) Circle partially drawn at t = 36s
(b) Circle completely drawn at t = 170s
(c) Top view showing the mobile manipulator trajectory
Fig 9 Manipulator robot interacting with a mobile manipulator in aworkspace with two cylindrical obstacles and a wall The parameters for thetrajectory generation were ωn = 01 ωd = 05 and dz = 01
End-users are agnostic to these infrastructural decisions Forthem usage performance and installation of the library matterthe most The usage of the library has been simplified byrelying on a programming syntax similar to the mathemati-
Listing 5 Main MATLAB code for the simulationsampling_time = 005total_time = 200for t=0sampling_timetotal_time
Get obstacles from V-REPplane = get_plane_from_vrep(virsquoObstaclePlanersquoDQk)cylinder1 = get_line_from_vrep(virsquoObstacleCylinder1rsquoDQk)cylinder2 = get_line_from_vrep(virsquoObstacleCylinder2rsquoDQk)
Set references for both robots[lwr4_xd lwr4_ff] = compute_lwr4_reference(lwr4
simulation_parameters lwr4_x0 t)[youbot_xd youbot_ff] = compute_youbot_reference(
youbot_control lwr4_xd lwr4_ff)
Compute the control input for the manipulatorlwr4_u = lwr4_controllercompute_tracking_control_signal(
lwr4_q vec8(lwr4_xd)vec8(lwr4_ff))
Compute constrained control input for the youbot[Jconstraint bconstraint] = compute_constraints(youbot
youbot_q planecylinder1 cylinder2)youbot_controlset_inequality_constraint(-Jconstraint
1bconstraint)youbot_u= youbot_controlcompute_tracking_control_signal(
youbot_q vec8(youbot_xd) vec8(youbot_ff))
Since we are using V-REP just for visualization integrate the control signal to update the robots configurationslwr4_q = lwr4_q + sampling_timelwr4_uyoubot_q = youbot_q + sampling_timeyoubot_u
Send desired values to V-REPlwr4_vreprobotsend_q_to_vrep(lwr4_q)youbot_vreprobotsend_q_to_vrep(youbot_q)end
cal description and being attentive to proper object-orientedprogramming practices Furthermore good performance isachieved by careful optimization of the library and by theC++ implementation
The ease of installation has been addressed in language-platform specific ways that we document in details on the DQRoboticsrsquo Read the Docs11
For MATLAB DQ Robotics is distributed as a Toolboxthat can be installed by the end-user in any of the MATLAB-compatible operating systems
For C++ we officially focus on Ubuntu long-term service(LTS) distributions in a similar way to the most recentdistributions of ROS and other large open-source libraries suchas TensorFlow12 For any Ubuntu LTS distribution that has notreached its end-of-life13 the user can install the C++ versionof the library with the following three commandssudo add-apt-repository ppadqrobotics -devreleasesudo apt-get updatesudo apt-get install libdqrobotics
made available via our release Personal Package Archive(PPA)14 in which we store the latest stable versions of thelibrary Interfaces between DQ Robotics and other librariesalso reside in the same PPA For example after addingthe PPA the user can install the V-REP interface with thefollowing commandsudo apt-get install libdqrobotics -interface -vrep
11httpsdqroboticsgithubioreadthedocsioenlatestinstallationhtml12httpswwwtensorfloworg13httpsubuntucomaboutrelease-cycle14httpslaunchpadnet~dqrobotics-dev+archiveubunturelease
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 10
The PPA guarantees that the user can reliably install thepackage in their system as the PPA only stores packages thatcompiled successfully Support for other operating systemswill be driven by user interest but should not be a bigchallenge for most systems since we are using only CMAKEand Eigen3 as project dependencies both of which are widelysupported
Lastly the Python version of DQ Robotics is made avail-able through a Pybind11-based15 Python wrapper of the C++library This has three main advantages First there is no needto redevelop and maintain versions of the libraries in differentprogramming languages which is highly demanding Secondunit-testing code written in Python which is easier to writeand maintain automatically validates the C++ library as wellThird we can have Pythonrsquos ease-of-use with C++rsquos perfor-mance for each function Although there is a computationaloverhead when using the Python bindings (in contrast withdirectly using the C++ code) it is much lower than a nativePython implementation A simple example of this can be seenin Table V in which we compare the average required timefor dual quaternion multiplications in the same machine
TABLE VCOMPUTATIONAL TIME OF DUAL-QUATERNION MULTIPLICATIONS
Mean [micros] Standard Deviation [micros]C++ (gcc 540) 022 00149
MATLAB R2019a 894 01223Python 352 Bindings 082 00242Python 352 Native 3142 01646
Mean and standard deviation of the required time for one dual-quaternion multiplicationThis was calculated from a thousand sets of a thousand executions on the same IntelCore i9 9900K Ubuntu 1604 x64 system and excludes the time required to generate therandom dual quaternions
An Ubuntu LTS user can install the Python3 version of DQRobotics from the Python Package Index (PyPI)16 with a singleline of code
python3 -m pip install --user dqrobotics
Lastly the PyPI package is continuously built from sourceusing TravisCI17 so that the code is properly compiled andtested before being distributed to our users
IX APPLICATIONS AND COMPARISON WITH OTHERPACKAGES AND LIBRARIES
DQ Robotics has been used for more than nine years tomodel and control different robots in applications such asbimanual surgical robots in constrained workspaces (Fig 10a)whole-body control of humanoid robots (Fig 10b) andthe decentralized formation control of mobile manipulators(Fig 10c)18
The main difference from other packages and librariesis that DQ Robotics implements dual quaternion algebra in
15httpsgithubcompybindpybind1116httpspypiorg17httpstravis-cicom18A more complete list of papers and applications can be found in https
dqroboticsgithubiocitationshtml
(a) The bimanual surgical robot SmartArm [12]
(b) Whole-body control of a humanoid robot [13]
(c) Cooperative mobile manipulators [14]
Fig 10 Examples of different applications of DQ Robotics
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 11
a close way to the mathematical notation Furthermore allsupported languages namely Python MATLAB and C++use the same convention and style and it is straightforwardto port code in one language to other supported languages Toenable that we avoid using features that are not available in allthree languages unless that would lead to poor-quality codeFrom the user point-of-view the advantage of such approachis that users can prototype their ideas on MATLAB usingthe convenient plot system then easily translate the code toC++ and deploy it on a real robotic platform The Pythonversion of the library is somewhere between MATLAB andC++ being suitable for prototyping and also for deploymenton real applications because C++ runs under the hood
In terms of target audience the MATLAB version of DQRobotics has some overlap with Peter Corkersquos Robotics Tool-box [15] most notably in terms of their use for educationand learning On the one hand earlier versions were greatlyinspired by Peter Corkersquos Robotics Toolbox most notablythe plot system which is particularly useful when teachingrobotics On the other hand DQ Robotics is entirely basedon general dual quaternion algebra whereas Robotics Tool-box uses classic representations for robot modeling althoughit does offer support for unit quaternions Moreover otherfunctionalities of Peter Corkersquos Robotics Toolbox such aspath planning localization and mapping are not available inDQ Robotics as of now However some functionalities ofPeter Corkersquos Robotics Toolbox are now available in toolboxesdistributed by MathWorks such as the Image ProcessingToolbox19 and the Robotics System Toolbox20 which are inprinciple compatible with DQ Robotics
Another MATLAB toolbox [7] focuses on the application ofunit dual quaternions to the neuroscience domain and presentsbasic kinematic modeling of serial mechanisms In addition tohaving many fewer functionalities than DQ Robotics it usesprocedural programming which makes the code quite differentfrom the notation used on paper
A major contrast between DQ Robotics and Peter Corkersquosand Leclercqrsquos MATLAB toolboxes is that DQ Robotics hasPython and C++ versions with identical APIs as far as theprogramming languages permit This increases the numberof possible users and considerably reduces the time requiredto go from prototyping to deployment Moreover this makesDQ Robotics compatible with powerful libraries available inPython such as SciPy21 and scikit-learn22 In addition bothPython and C++ versions of the library can be readily usedalongside ROS which is the current standard of shareablecode in the robotics community Lastly a modern developmentinfrastructure not available in other existing libraries makesthe library scalable and easier to maintain and extend
X CONCLUSIONS
This paper presented DQ Robotics a comprehensive com-putational library for robot modeling and control using dual
19httpswwwmathworkscomproductsimagehtml20httpswwwmathworkscomproductsroboticshtml21httpswwwscipyorg22httpsscikit-learnorgstable
quaternion algebra It has the unique feature of using a notationvery close to the mathematical description and supportingthree different programming languages namely MATLABPython and C++ while using very similar conventions amongthem making it very easy to switch between languages andthus shortening the development time from prototyping tothe implementation on actual robots The library currentlysupports serial manipulator robots cooperative systems suchas two-arm robots mobile manipulators and even branchedmechanisms such as humanoids Robot models are generatedautomatically from simple geometrical parameters such asthe Denavit-Hartenberg parameters In addition robots canbe easily combined to yield more complex ones while thecorresponding forward kinematics and differential kinematicsare also computed automatically and analytically at executiontime The library also offers a rich set of motion controllersand it is very simple to implement new ones Although it doesnot offer dynamics modeling yet it can be easily integrated toexisting libraries for that purpose However robot dynamicsmodeling using dual quaternion algebra is currently underdevelopment and will be integrated into the library as soonas it becomes mature Rather than being a competitor toexisting libraries DQ Robotics aims at complementing themand helping in the popularization of dual quaternion algebrain the robotics domain
ACKNOWLEDGMENT
The authors would like to thank all users of the DQ Roboticslibrary for their valuable feedback and bug reports speciallyJuan Joseacute Quiroz-Omantildea and other members of the MACROresearch group at UFMG
REFERENCES
[1] O Bottema and B Roth Theoretical kinematics North-HollandPublishing Company 1979 vol 24
[2] J McCarthy Introduction to theoretical kinematics 1st ed The MITPress 1990
[3] B V Adorno ldquoTwo-arm Manipulation From Manipulators to EnhancedHuman-Robot Collaboration [Contribution agrave la manipulation agrave deuxbras des manipulateurs agrave la collaboration homme-robot]rdquo PhDDissertation Universiteacute Montpellier 2 2011 [Online] Availablehttpstelarchives-ouvertesfrtel-00641678
[4] M W Spong and M Vidyasagar Robot dynamics and control JohnWiley amp Sons 2008
[5] B Siciliano L Sciavicco L Villani and G Oriolo Robotics modellingplanning and control Springer Science amp Business Media 2010
[6] B Siciliano and O Khatib Springer handbook of robotics Springer2016
[7] G Leclercq P Lefegravevre and G Blohm ldquo3D kinematics using dualquaternions theory and applications in neurosciencerdquo Frontiers inBehavioral Neuroscience vol 7 no February p 7 jan 2013 [Online]Available httpwwwncbinlmnihgovpubmed23443667httpjournalfrontiersinorgarticle103389fnbeh201300007abstract
[8] B V Adorno ldquoRobot Kinematic Modeling and Control Based on DualQuaternion Algebra ndash Part I Fundamentalsrdquo 2017 [Online] Availablehttpshalarchives-ouvertesfrhal-01478225v1
[9] M Quigley K Conley B Gerkey J Faust T Foote J LeibsR Wheeler and A Y Ng ldquoRos an open-source robot operating systemrdquoin ICRA workshop on open source software vol 3 no 32 Kobe Japan2009 p 5
[10] E Rohmer S P N Singh and M Freese ldquoV-REP A versatile andscalable robot simulation frameworkrdquo in 2013 IEEERSJ InternationalConference on Intelligent Robots and Systems IEEE nov 2013
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 12
[11] M M Marinho B V Adorno K Harada and M Mitsuishi ldquoDynamicActive Constraints for Surgical Robots Using Vector-Field InequalitiesrdquoIEEE Transactions on Robotics vol 35 no 5 pp 1166ndash1185 oct 2019[Online] Available httpsieeexploreieeeorgdocument8742769
[12] M M Marinho K Harada A Morita and M Mitsuishi ldquoSmartarmIntegration and validation of a versatile surgical robotic system forconstrained workspacesrdquo The International Journal of Medical Roboticsand Computer Assisted Surgery (in press) 2019
[13] J J Quiroz-Omana and B V Adorno ldquoWhole-Body Control With(Self) Collision Avoidance Using Vector Field Inequalitiesrdquo IEEERobotics and Automation Letters vol 4 no 4 pp 4048ndash4053 oct 2019
[Online] Available httpsieeexploreieeeorgdocument8763977[14] H J Savino L C Pimenta J A Shah and B V Adorno
ldquoPose consensus based on dual quaternion algebra with applicationto decentralized formation control of mobile manipulatorsrdquo Journalof the Franklin Institute pp 1ndash36 oct 2019 [Online] AvailablehttpslinkinghubelseviercomretrievepiiS0016003219307161
[15] P Corke Robotics Vision and Control 2nd ed ser SpringerTracts in Advanced Robotics Springer International Publishing2017 vol 118 [Online] Available httplinkspringercom101007978-3-319-54413-7
- I Introduction
-
- I-A How to follow the tutorial
- I-B Notation choice and common operations
-
- II Representing rigid motions
- III Robot kinematics
- IV Geometric Primitives
- V Robot motion control
- VI Interface with ROS
- VII Interface with V-REP
-
- VII-A A more complete example
-
- VIII Development infrastructure
- IX Applications and comparison with other packages and libraries
- X Conclusions
- References
-
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 5
TABLE IIMAIN METHODS OF CLASS DQ_Kinematics AND ITS SUBCLASSES
DQ_Kinematicsfkm Compute the forward kinematicspose_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of the pose of a frame attached to
the robotset_base_frame Set the physical location of the robot in spaceset_reference_frame Set the reference frame for all calculationsline_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of a line attached to the robotplane_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of a plane attached to the robotrotation_jacobian Compute only the rotational part of the pose_jacobiantranslation_jacobian Compute only the translational part of the pose_jacobianprim_dist_jacobian Compute the Jacobian that maps the configurations velocities to the time derivative of the distance between a
primitive attached to the robot and another primitive in the workspace For instance prim_dist_jacobian can beline_to_line_distance_jacobian line_to_point_distance_jacobianplane_to_point_distance_jacobian point_to_line_distance_jacobianpoint_to_plane_distance_jacobian point_to_point_distance_jacobian
DQ_SerialManipulatorpose_jacobian_derivative Compute the analytical time derivative of the pose Jacobian matrixset_effector Set a constant transformation for the end-effector pose with respect to the frame attached to the end of the last link
DQ_MobileBase DQ_HolonomicBase and DQ_DifferentialDriveRobotset_frame_displacement Set the rigid transformation for the base frameset_base_diameter Change the base diameterconstraint_jacobian Compute the Jacobian that relates the wheels velocities to the configuration velocities
DQ_WholeBodyadd Add a new element to the end of the serially coupled kinematic chainadd_reversed Add a new element but in reverse order to the end of the serially coupled kinematic chainget_chain Returns the complete kinematic chainsequential Reorganize a sequential configuration vector in the ordering required by each kinematic chain (ie the vector blocks
corresponding to reversed chains are reversed)
DQ_CooperativeDualTaskSpaceabsolute_pose Compute the pose of a frame between the two end-effectors which is usually related to a grasped objectabsolute_pose_jacobian Compute the Jacobian that maps the joint velocities of the two-arm system to the time derivative of the absolute posepose1 and pose2 Compute the poses of the first and second end-effectors respectivelypose_jacobian1 andpose_jacobian2
Compute the Jacobians that maps the configurations velocities to the time derivative of the poses of the first andsecond end-effectors respectively
relative_pose Compute the rigid transformation between the two end-effectorsrelative_pose_jacobian Compute the Jacobian that maps the joint velocities of the two-arm system to the time derivative of the relative pose
Abstract methods are written in italics concrete methods are written in upright and static methods are underlined Concrete methods that implement theirabstract counterparts are omitted for the sake of conciseness
Fig 5 Two geometric primitives represented by elements of the dualquaternion algebra (a) the blue plane πa represented by the normal na
and the distance to the origin da = 〈pana〉 where pa is an arbitrary pointon the plane (b) and the green line la represented by the direction la andthe moment sa times la where sa is an arbitrary point on the line
Fig 6 MATLAB plot reference frame Fa line la = + ε(ıtimes ) parallelto the y-axis and passing through point (1 0 0) and plane πa = + ε15with normal parallel to the y-axis and whose distance from the origin of Fa
is 15 m
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 6
from the dual quaternion On the other hand the position iseasily extracted from the HTM but to obtain the orientationone usually has to extract the rotation angle and the rotationaxis from the rotation matrix This extraction introduces rep-resentational singularities when the rotation angle equals zeroFurthermore when designing constrained controllers based ongeometrical constraints one has to represent several geometri-cal primitives in different coordinate systems and in differentlocations Dual quaternion algebra is particularly useful in thiscase because several relevant geometrical primitives are easilyrepresented as dual quaternions as shown in Section IV
Users can implement their own controllers in DQ RoboticsFor instance suppose that the desired end-effector pose isgiven by Listing 1 and the initial robot configuration andcontroller parameters are given in Listing 2
Listing 2 Parameters for a simple kinematic controllerq = [0 03770 01257 -05655 0 0 0]rsquoT = 0001 sampling timegain = 10 controller gain
A classic controller based on an Euclidean error function andthe Jacobian pseudo-inverse can be implemented in MATLABas shown in Listing 3
Listing 3 Kinematic controller based on the Jacobian pseudoinversee = ones(81) initialize the error vectorwhile norm(e) gt 0001
J = lwr4pose_jacobian(q)x = lwr4fkm(q)e = vec8(x-xd)u = -pinv(J)gaineq = q + Tu
end
In Listing 3 when executing the controller on a real robot thecontrol input u is usually sent directly to the robot
There are several kinematic controllers included in DQRobotics including the one in Listing 3 The class hierarchyfor the kinematic controllers is summarized by the simplifiedUML diagram in Fig 7 All kinematic controller classes inheritfrom the abstract superclass DQ_KinematicController andtheir main methods are detailed in Table III
The DQ_KinematicController subclasses are focused onthe control structure not on particular geometrical tasks Thesame subclass can be used regardless if the goal is to controlthe end-effector pose position orientation etc In order todistinguish between task objectives it suffices to usecontrollerset_control_objective(GOAL)
where GOAL is an object from the enumeration classControlObjective which currently provides the follow-ing enumeration members Distance DistanceToPlaneLine Plane Pose Rotation and Translation Thereforethe implementation in Listing 3 can be rewritten using theDQ_PseudoinverseController class as
Listing 4 Controller based on the DQ_PseudoinverseController classcontrol = DQ_PseudoinverseController(lwr4)controlset_control_objective(ControlObjectivePose)
controlset_gain(gain)controlset_stability_threshold(00001)while ~controlsystem_reached_stable_region()
Fig 7 Simplified UML class diagram of the robot kinematic controller classeswith the main methods that are available in C++ Python and MATLAB
u = controlcompute_setpoint_control_signal(qvec8(xd))q = q + Tu
end
Furthermore if the geometric task objective changes theDQ_KinematicController subclass is responsible for calcu-lating the appropriate forward kinematics and the correspond-ing Jacobian For instance if a line is attached to the end-effector and the goal is to align it with a line in the workspacethen it suffices to replace the second line of Listing 4 bycontrolset_primitive_to_effector(line)controlset_control_objective(ControlObjectiveLine)
where line is a dual quaternion corresponding to a linepassing through the origin of the end-effector frame
Furthermore since any subclass ofDQ_KinematicController provides the methods usedin Listing 4 changing the controller is a matter of changingjust one line of code namely the first one in Listing 4
VI INTERFACE WITH ROSThe Robot Operating System (ROS) [9] is widely used
by the robotics community After the one-line installationdescribed in Section VIII the Python version of the DQRobotics library can be imported in any Python script in thesystem therefore it is compatible with ROS out-of-the-boxMATLAB also has an interface with ROS7
For C++ code in its most recent versions ROS uses thecatkin_build8 environment which itself depends on CMAKEGiven that DQ Robotics C++ is installed as a system library inUbuntu as shown in Section VIII ROS users can readily haveaccess to the library as they would to any other system librarywith a proper CMAKE configuration After installation theC++ version of the library can be linked with
7httpswwwmathworkscomhelprosugget-started-with-roshtml8httpscatkin-toolsreadthedocsioenlatestverbscatkin_buildhtml
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 7
TABLE IIIMAIN METHODS OF CLASS DQ_KinematicController AND ITS MAIN SUBCLASSES
DQ_KinematicControllercompute_setpoint_control_signal Compute the control input to regulate to a set-pointcompute_tracking_control_signal Compute the control input to track a trajectoryget_control_objective Return the control objectiveget_jacobian Return the correct Jacobian based on the control objectiveget_task_variable Return the task variable based on the control objectiveis_set Verify if the controller is set and ready to be usedset_control_objective Set the control objective using predefined goals in ControlObjectiveset_damping Set the damping to prevent instabilities near singular configurationsset_gain Set the controller gainset_primitive_to_effector Attach primitive (eg plane line point) to the end-effectorset_stability_threshold Set the threshold that determines if a stable region has been reachedsystem_reached_stable_region Return true if the trajectories of the closed-loop system have reached a stable region (ie a
positive invariant set) false otherwiseverify_stability Verify if the closed-loop region has reached a stable region
DQ_PseudoinverseSetpointControllercompute_tracking_control_signal Given the task error x = xminus xd isin Rn and the feedforward term xd compute the control
signal u = J+ (minusλx+ xd) where J+ is the pseudoinverse of the task Jacobian J and λis the controller gain
DQ_KinematicConstrainedControllerset_equality_constraint Add equality constraints of type Bq = b where q isin Rn is the vector of joint velocities
b isin Rm is the vector of equality constraints and B isin Rmtimesnset_inequality_constraint Add inequality constraints of type Aq a where q isin Rn is the vector of joint velocities
a isin Rm is the vector of inequality constraints and A isin Rmtimesn
DQ_TaskspaceQuadraticProgrammingControllercompute_objective_function_linear_component Compute the vector h used in the objective function (12)qTHq + hT qcompute_objective_function_symmetric_matrix Compute the matrix H used in the objective function (12)qTHq + hT qcompute_tracking_control_signal Given the task error x = xminus xd isin Rn compute the control signal given by
u isin argminq(12)qTHq + hT q subject to Aq a and Bq = b
DQ_ClassicQPControllercompute_objective_function_linear_component Compute the vector h used in (12)qTHq + hT q = Jq + λxminus xd22compute_objective_function_symmetric_matrix Compute the matrix H used in (12)qTHq + hT q = Jq + λxminus xd22
Abstract methods are written in italics and concrete methods are written in upright In all concrete classes the methodcompute_setpoint_control_signal() does not include the feedforward term in the method parameters and is equivalent to the methodcompute_tracking_control_signal with xd = 0 Therefore they are omitted for the sake of conciseness
target_link_libraries(my_binary dqrobotics)
for a given binary called my_binary
VII INTERFACE WITH V-REPDQ Robotics provides a simple interface to V-REP [10]
enabling users to develop complex simulations without havingto delve into the V-REP documentation The V-REP interfacecomes bundled with the MATLAB and Python versions of thelibrary The C++ version of our V-REP interface comes asa separate package and the following CMAKE directive willlink the required shared objecttarget_link_libraries(my_binary
dqroboticsdqrobotics -vrep-interface)
for a given my_binaryThe available methods are shown in Table IV
To start the communication with V-REP using its remoteAPI9 only two methods are necessaryvi = DQ_VrepInterfaceviconnect(rsquo127001rsquo19997)vistart_simulation()
9httpwwwcoppeliaroboticscomhelpFilesenlegacyRemoteApiOverviewhtm
TABLE IVVrepInterface
connect Connect to a V-REP Remote API Serverdisconnect Disconnect from currently connected serverdisconnect_all Flush all Remote API connectionsstart_simulation Start V-REP simulationstop_simulation Stop V-REP simulationget_object_translation Get object translation as a pure quaternionset_object_translation Set object translation with a pure quaternionget_object_rotation Get object rotation as a unit quaternionset_object_rotation Set object rotation with a unit quaternionget_object_pose Get object pose as a unit dual quaternionset_object_pose Set object pose with a unit dual quaternionset_joint_positions Set the joint positions of a manipulator robotset_joint_target_positions Set the joint target positions of a manipulator
robotget_joint_positions Get the joint positions of a manipulator robot
The method start_simulation starts the V-REP simulationwith the default asynchronous mode and the recommended 5ms communication thread cycle10
Since each robot joint in V-REP is associated with a nameit is convenient to encapsulate this information inside the robotclass Therefore classes implementing robots that use the V-
10For other parameters refer to the code
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 8
REP interface must be a subclass of DQ_VrepRobot to pro-vide the methods send_q_to_vrep and get_q_from_vrepwhich are used to send the desired robot configuration vectorto V-REP and get the current robot configuration vector fromV-REP respectively
Objects poses in a V-REP scene can be retrievedor set by using the methods get_object_pose andset_object_pose respectively which are useful when de-signing motion planners or controllers that take into accountthe constraints imposed by obstacles Finally in order to endthe V-REP simulation it suffices to use two methodsvistop_simulation()vidisconnect()
A A more complete example
To better highlight how DQ Robotics can be used withV-REP in this example a KUKA LWR4 manipulator robotinteracts with a KUKA YouBot mobile manipulator in aworkspace containing three obstacles namely two cylindersand a plane as shown in Fig 8
Fig 8 A more complete example The reference frame is given by F thetime-varying frame Fc is used to indicate where the circle must be drawn inthe whiteboard and Fmm is the end-effector frame of the mobile manipulatorObstacles are represented as geometrical primitives the wall is representedby the plane π = nπ + ε〈pπ nπ〉 where pπ is an arbitrary point onthe plane and nπ is the plane normal and the two cylindrical obstaclesare represented by (l1 R1) and (l2 R2) where li = li + εpi times li isthe ith cylinder centerline and Ri is the corresponding cylinder radius withi isin 1 2 Furthermore li is the line direction and pi is an arbitrary pointon the ith line To prevent collisions between the robot and the obstacles therobot is represented by a circle given by (p3 R3) where p3 and R3 are thecenter and radius of the circle respectively
The manipulator robot holds a whiteboard and uses apseudo-inverse-based controller with a feedforward term totrack a trajectory given by xm(t) = rm(t)xm(0)pm(t)where rm = cos(φ(t)2) + k sin(φ(t)2) with φ(t) =(π2) sin(ωnt) and p = 1 + (12)εdz cos(ωdt)k Viewedfrom the top the whiteboard will follow a semi-circular pathwith a radial oscillation at a frequency of ωd rads andamplitude dzm and an oscillation around the vertical axisat a frequency of ωn rads The mobile manipulator holds afelt pen and follows the manipulator trajectory while drawing a
circle on the whiteboard The mobile manipulator desired end-effector trajectory is given by xmm(t) = xm(t)xc wherexc = 1 + (12)ε0015k is a constant displacement of 15cmalong the z-axis to account for the whiteboard width and = cos(π2) + sin(π2) is a rotation of π around they-axis so that both end-effectors have their z-axis pointingto opposite directions To prevent collision with obstaclesit uses a constrained controller in which for each obstaclewe use a differential inequality to ensure that the robot willapproach it in the worst case with an exponential velocitydecrease For each obstacle we define a distance metricd (t) d (t)minus dsafe where dsafe isin [0infin) is an arbitraryconstant safe distance and the following inequalities must holdfor all t
˙d (t) ge minusηdd (t)lArrrArr minusJdq le ηdd (t) (2)
where ηd isin [0infin) is used to adjust the approach velocity andJd is the distance Jacobian related to the obstacle [11] Thelower is ηd the lower is the allowed approach velocity
In the case of our example to prevent collisions betweenthe robot and the wall we let dsafe R3 (see Fig 8) suchthat d (t) = 0 lArrrArr d (t) = R3 and the distance function isdefined as the distance between p3 which is the center of thecircle enclosing the robot and the wall plane π Analogouslyto prevent collisions between the robot and the static cylinderswe use one inequality such as (2) for each cylinder wheredsafe Ri + R3 and d(t) is defined as the distance betweenp3 and the lines li for i isin 1 2 Although those distancefunctions and corresponding Jacobians can be computed byusing the classes DQ_Geometry and DQ_Kinematics respec-tively details of how they are obtained using dual quaternionalgebra are presented in [11]
Whenever the felt-tip is sufficiently close to the whiteboardit starts to draw a circle Figs 9a and 9b show that the mobilemanipulator successfully draws the circle although there aresome small imperfections as the obstacle avoidance is a hardconstraint that is always respected at the expense of worsetrajectory tracking Last Fig 9c shows the mobile manipulatortrajectory which is always adapted to prevent collisions withthe cylinders and the wall
The main MATLAB code is shown in Listing 5 For the sakeof conciseness the functions compute_lwr4_referenceand compute_youbot_reference which calculate theaforementioned end-effector trajectories are omittedThe same applies for get_plane_from_vrep andget_line_from_vrep which get information about thelocation of the corresponding geometric primitives inV-REP and compute_constraints which calculatesconstraints such as (2) for each obstacle in the scene Theoriginal source code used in this example is available assupplementary material For a more up-to-date version referto httpsgithubcomdqroboticsmatlab-examples
VIII DEVELOPMENT INFRASTRUCTURE
Aiming at the scalability of the DQ Robotics library andtaking advantage of the familiarity that current developers havewith Git and Github we opted to also use those services
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 9
(a) Circle partially drawn at t = 36s
(b) Circle completely drawn at t = 170s
(c) Top view showing the mobile manipulator trajectory
Fig 9 Manipulator robot interacting with a mobile manipulator in aworkspace with two cylindrical obstacles and a wall The parameters for thetrajectory generation were ωn = 01 ωd = 05 and dz = 01
End-users are agnostic to these infrastructural decisions Forthem usage performance and installation of the library matterthe most The usage of the library has been simplified byrelying on a programming syntax similar to the mathemati-
Listing 5 Main MATLAB code for the simulationsampling_time = 005total_time = 200for t=0sampling_timetotal_time
Get obstacles from V-REPplane = get_plane_from_vrep(virsquoObstaclePlanersquoDQk)cylinder1 = get_line_from_vrep(virsquoObstacleCylinder1rsquoDQk)cylinder2 = get_line_from_vrep(virsquoObstacleCylinder2rsquoDQk)
Set references for both robots[lwr4_xd lwr4_ff] = compute_lwr4_reference(lwr4
simulation_parameters lwr4_x0 t)[youbot_xd youbot_ff] = compute_youbot_reference(
youbot_control lwr4_xd lwr4_ff)
Compute the control input for the manipulatorlwr4_u = lwr4_controllercompute_tracking_control_signal(
lwr4_q vec8(lwr4_xd)vec8(lwr4_ff))
Compute constrained control input for the youbot[Jconstraint bconstraint] = compute_constraints(youbot
youbot_q planecylinder1 cylinder2)youbot_controlset_inequality_constraint(-Jconstraint
1bconstraint)youbot_u= youbot_controlcompute_tracking_control_signal(
youbot_q vec8(youbot_xd) vec8(youbot_ff))
Since we are using V-REP just for visualization integrate the control signal to update the robots configurationslwr4_q = lwr4_q + sampling_timelwr4_uyoubot_q = youbot_q + sampling_timeyoubot_u
Send desired values to V-REPlwr4_vreprobotsend_q_to_vrep(lwr4_q)youbot_vreprobotsend_q_to_vrep(youbot_q)end
cal description and being attentive to proper object-orientedprogramming practices Furthermore good performance isachieved by careful optimization of the library and by theC++ implementation
The ease of installation has been addressed in language-platform specific ways that we document in details on the DQRoboticsrsquo Read the Docs11
For MATLAB DQ Robotics is distributed as a Toolboxthat can be installed by the end-user in any of the MATLAB-compatible operating systems
For C++ we officially focus on Ubuntu long-term service(LTS) distributions in a similar way to the most recentdistributions of ROS and other large open-source libraries suchas TensorFlow12 For any Ubuntu LTS distribution that has notreached its end-of-life13 the user can install the C++ versionof the library with the following three commandssudo add-apt-repository ppadqrobotics -devreleasesudo apt-get updatesudo apt-get install libdqrobotics
made available via our release Personal Package Archive(PPA)14 in which we store the latest stable versions of thelibrary Interfaces between DQ Robotics and other librariesalso reside in the same PPA For example after addingthe PPA the user can install the V-REP interface with thefollowing commandsudo apt-get install libdqrobotics -interface -vrep
11httpsdqroboticsgithubioreadthedocsioenlatestinstallationhtml12httpswwwtensorfloworg13httpsubuntucomaboutrelease-cycle14httpslaunchpadnet~dqrobotics-dev+archiveubunturelease
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 10
The PPA guarantees that the user can reliably install thepackage in their system as the PPA only stores packages thatcompiled successfully Support for other operating systemswill be driven by user interest but should not be a bigchallenge for most systems since we are using only CMAKEand Eigen3 as project dependencies both of which are widelysupported
Lastly the Python version of DQ Robotics is made avail-able through a Pybind11-based15 Python wrapper of the C++library This has three main advantages First there is no needto redevelop and maintain versions of the libraries in differentprogramming languages which is highly demanding Secondunit-testing code written in Python which is easier to writeand maintain automatically validates the C++ library as wellThird we can have Pythonrsquos ease-of-use with C++rsquos perfor-mance for each function Although there is a computationaloverhead when using the Python bindings (in contrast withdirectly using the C++ code) it is much lower than a nativePython implementation A simple example of this can be seenin Table V in which we compare the average required timefor dual quaternion multiplications in the same machine
TABLE VCOMPUTATIONAL TIME OF DUAL-QUATERNION MULTIPLICATIONS
Mean [micros] Standard Deviation [micros]C++ (gcc 540) 022 00149
MATLAB R2019a 894 01223Python 352 Bindings 082 00242Python 352 Native 3142 01646
Mean and standard deviation of the required time for one dual-quaternion multiplicationThis was calculated from a thousand sets of a thousand executions on the same IntelCore i9 9900K Ubuntu 1604 x64 system and excludes the time required to generate therandom dual quaternions
An Ubuntu LTS user can install the Python3 version of DQRobotics from the Python Package Index (PyPI)16 with a singleline of code
python3 -m pip install --user dqrobotics
Lastly the PyPI package is continuously built from sourceusing TravisCI17 so that the code is properly compiled andtested before being distributed to our users
IX APPLICATIONS AND COMPARISON WITH OTHERPACKAGES AND LIBRARIES
DQ Robotics has been used for more than nine years tomodel and control different robots in applications such asbimanual surgical robots in constrained workspaces (Fig 10a)whole-body control of humanoid robots (Fig 10b) andthe decentralized formation control of mobile manipulators(Fig 10c)18
The main difference from other packages and librariesis that DQ Robotics implements dual quaternion algebra in
15httpsgithubcompybindpybind1116httpspypiorg17httpstravis-cicom18A more complete list of papers and applications can be found in https
dqroboticsgithubiocitationshtml
(a) The bimanual surgical robot SmartArm [12]
(b) Whole-body control of a humanoid robot [13]
(c) Cooperative mobile manipulators [14]
Fig 10 Examples of different applications of DQ Robotics
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 11
a close way to the mathematical notation Furthermore allsupported languages namely Python MATLAB and C++use the same convention and style and it is straightforwardto port code in one language to other supported languages Toenable that we avoid using features that are not available in allthree languages unless that would lead to poor-quality codeFrom the user point-of-view the advantage of such approachis that users can prototype their ideas on MATLAB usingthe convenient plot system then easily translate the code toC++ and deploy it on a real robotic platform The Pythonversion of the library is somewhere between MATLAB andC++ being suitable for prototyping and also for deploymenton real applications because C++ runs under the hood
In terms of target audience the MATLAB version of DQRobotics has some overlap with Peter Corkersquos Robotics Tool-box [15] most notably in terms of their use for educationand learning On the one hand earlier versions were greatlyinspired by Peter Corkersquos Robotics Toolbox most notablythe plot system which is particularly useful when teachingrobotics On the other hand DQ Robotics is entirely basedon general dual quaternion algebra whereas Robotics Tool-box uses classic representations for robot modeling althoughit does offer support for unit quaternions Moreover otherfunctionalities of Peter Corkersquos Robotics Toolbox such aspath planning localization and mapping are not available inDQ Robotics as of now However some functionalities ofPeter Corkersquos Robotics Toolbox are now available in toolboxesdistributed by MathWorks such as the Image ProcessingToolbox19 and the Robotics System Toolbox20 which are inprinciple compatible with DQ Robotics
Another MATLAB toolbox [7] focuses on the application ofunit dual quaternions to the neuroscience domain and presentsbasic kinematic modeling of serial mechanisms In addition tohaving many fewer functionalities than DQ Robotics it usesprocedural programming which makes the code quite differentfrom the notation used on paper
A major contrast between DQ Robotics and Peter Corkersquosand Leclercqrsquos MATLAB toolboxes is that DQ Robotics hasPython and C++ versions with identical APIs as far as theprogramming languages permit This increases the numberof possible users and considerably reduces the time requiredto go from prototyping to deployment Moreover this makesDQ Robotics compatible with powerful libraries available inPython such as SciPy21 and scikit-learn22 In addition bothPython and C++ versions of the library can be readily usedalongside ROS which is the current standard of shareablecode in the robotics community Lastly a modern developmentinfrastructure not available in other existing libraries makesthe library scalable and easier to maintain and extend
X CONCLUSIONS
This paper presented DQ Robotics a comprehensive com-putational library for robot modeling and control using dual
19httpswwwmathworkscomproductsimagehtml20httpswwwmathworkscomproductsroboticshtml21httpswwwscipyorg22httpsscikit-learnorgstable
quaternion algebra It has the unique feature of using a notationvery close to the mathematical description and supportingthree different programming languages namely MATLABPython and C++ while using very similar conventions amongthem making it very easy to switch between languages andthus shortening the development time from prototyping tothe implementation on actual robots The library currentlysupports serial manipulator robots cooperative systems suchas two-arm robots mobile manipulators and even branchedmechanisms such as humanoids Robot models are generatedautomatically from simple geometrical parameters such asthe Denavit-Hartenberg parameters In addition robots canbe easily combined to yield more complex ones while thecorresponding forward kinematics and differential kinematicsare also computed automatically and analytically at executiontime The library also offers a rich set of motion controllersand it is very simple to implement new ones Although it doesnot offer dynamics modeling yet it can be easily integrated toexisting libraries for that purpose However robot dynamicsmodeling using dual quaternion algebra is currently underdevelopment and will be integrated into the library as soonas it becomes mature Rather than being a competitor toexisting libraries DQ Robotics aims at complementing themand helping in the popularization of dual quaternion algebrain the robotics domain
ACKNOWLEDGMENT
The authors would like to thank all users of the DQ Roboticslibrary for their valuable feedback and bug reports speciallyJuan Joseacute Quiroz-Omantildea and other members of the MACROresearch group at UFMG
REFERENCES
[1] O Bottema and B Roth Theoretical kinematics North-HollandPublishing Company 1979 vol 24
[2] J McCarthy Introduction to theoretical kinematics 1st ed The MITPress 1990
[3] B V Adorno ldquoTwo-arm Manipulation From Manipulators to EnhancedHuman-Robot Collaboration [Contribution agrave la manipulation agrave deuxbras des manipulateurs agrave la collaboration homme-robot]rdquo PhDDissertation Universiteacute Montpellier 2 2011 [Online] Availablehttpstelarchives-ouvertesfrtel-00641678
[4] M W Spong and M Vidyasagar Robot dynamics and control JohnWiley amp Sons 2008
[5] B Siciliano L Sciavicco L Villani and G Oriolo Robotics modellingplanning and control Springer Science amp Business Media 2010
[6] B Siciliano and O Khatib Springer handbook of robotics Springer2016
[7] G Leclercq P Lefegravevre and G Blohm ldquo3D kinematics using dualquaternions theory and applications in neurosciencerdquo Frontiers inBehavioral Neuroscience vol 7 no February p 7 jan 2013 [Online]Available httpwwwncbinlmnihgovpubmed23443667httpjournalfrontiersinorgarticle103389fnbeh201300007abstract
[8] B V Adorno ldquoRobot Kinematic Modeling and Control Based on DualQuaternion Algebra ndash Part I Fundamentalsrdquo 2017 [Online] Availablehttpshalarchives-ouvertesfrhal-01478225v1
[9] M Quigley K Conley B Gerkey J Faust T Foote J LeibsR Wheeler and A Y Ng ldquoRos an open-source robot operating systemrdquoin ICRA workshop on open source software vol 3 no 32 Kobe Japan2009 p 5
[10] E Rohmer S P N Singh and M Freese ldquoV-REP A versatile andscalable robot simulation frameworkrdquo in 2013 IEEERSJ InternationalConference on Intelligent Robots and Systems IEEE nov 2013
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 12
[11] M M Marinho B V Adorno K Harada and M Mitsuishi ldquoDynamicActive Constraints for Surgical Robots Using Vector-Field InequalitiesrdquoIEEE Transactions on Robotics vol 35 no 5 pp 1166ndash1185 oct 2019[Online] Available httpsieeexploreieeeorgdocument8742769
[12] M M Marinho K Harada A Morita and M Mitsuishi ldquoSmartarmIntegration and validation of a versatile surgical robotic system forconstrained workspacesrdquo The International Journal of Medical Roboticsand Computer Assisted Surgery (in press) 2019
[13] J J Quiroz-Omana and B V Adorno ldquoWhole-Body Control With(Self) Collision Avoidance Using Vector Field Inequalitiesrdquo IEEERobotics and Automation Letters vol 4 no 4 pp 4048ndash4053 oct 2019
[Online] Available httpsieeexploreieeeorgdocument8763977[14] H J Savino L C Pimenta J A Shah and B V Adorno
ldquoPose consensus based on dual quaternion algebra with applicationto decentralized formation control of mobile manipulatorsrdquo Journalof the Franklin Institute pp 1ndash36 oct 2019 [Online] AvailablehttpslinkinghubelseviercomretrievepiiS0016003219307161
[15] P Corke Robotics Vision and Control 2nd ed ser SpringerTracts in Advanced Robotics Springer International Publishing2017 vol 118 [Online] Available httplinkspringercom101007978-3-319-54413-7
- I Introduction
-
- I-A How to follow the tutorial
- I-B Notation choice and common operations
-
- II Representing rigid motions
- III Robot kinematics
- IV Geometric Primitives
- V Robot motion control
- VI Interface with ROS
- VII Interface with V-REP
-
- VII-A A more complete example
-
- VIII Development infrastructure
- IX Applications and comparison with other packages and libraries
- X Conclusions
- References
-
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 6
from the dual quaternion On the other hand the position iseasily extracted from the HTM but to obtain the orientationone usually has to extract the rotation angle and the rotationaxis from the rotation matrix This extraction introduces rep-resentational singularities when the rotation angle equals zeroFurthermore when designing constrained controllers based ongeometrical constraints one has to represent several geometri-cal primitives in different coordinate systems and in differentlocations Dual quaternion algebra is particularly useful in thiscase because several relevant geometrical primitives are easilyrepresented as dual quaternions as shown in Section IV
Users can implement their own controllers in DQ RoboticsFor instance suppose that the desired end-effector pose isgiven by Listing 1 and the initial robot configuration andcontroller parameters are given in Listing 2
Listing 2 Parameters for a simple kinematic controllerq = [0 03770 01257 -05655 0 0 0]rsquoT = 0001 sampling timegain = 10 controller gain
A classic controller based on an Euclidean error function andthe Jacobian pseudo-inverse can be implemented in MATLABas shown in Listing 3
Listing 3 Kinematic controller based on the Jacobian pseudoinversee = ones(81) initialize the error vectorwhile norm(e) gt 0001
J = lwr4pose_jacobian(q)x = lwr4fkm(q)e = vec8(x-xd)u = -pinv(J)gaineq = q + Tu
end
In Listing 3 when executing the controller on a real robot thecontrol input u is usually sent directly to the robot
There are several kinematic controllers included in DQRobotics including the one in Listing 3 The class hierarchyfor the kinematic controllers is summarized by the simplifiedUML diagram in Fig 7 All kinematic controller classes inheritfrom the abstract superclass DQ_KinematicController andtheir main methods are detailed in Table III
The DQ_KinematicController subclasses are focused onthe control structure not on particular geometrical tasks Thesame subclass can be used regardless if the goal is to controlthe end-effector pose position orientation etc In order todistinguish between task objectives it suffices to usecontrollerset_control_objective(GOAL)
where GOAL is an object from the enumeration classControlObjective which currently provides the follow-ing enumeration members Distance DistanceToPlaneLine Plane Pose Rotation and Translation Thereforethe implementation in Listing 3 can be rewritten using theDQ_PseudoinverseController class as
Listing 4 Controller based on the DQ_PseudoinverseController classcontrol = DQ_PseudoinverseController(lwr4)controlset_control_objective(ControlObjectivePose)
controlset_gain(gain)controlset_stability_threshold(00001)while ~controlsystem_reached_stable_region()
Fig 7 Simplified UML class diagram of the robot kinematic controller classeswith the main methods that are available in C++ Python and MATLAB
u = controlcompute_setpoint_control_signal(qvec8(xd))q = q + Tu
end
Furthermore if the geometric task objective changes theDQ_KinematicController subclass is responsible for calcu-lating the appropriate forward kinematics and the correspond-ing Jacobian For instance if a line is attached to the end-effector and the goal is to align it with a line in the workspacethen it suffices to replace the second line of Listing 4 bycontrolset_primitive_to_effector(line)controlset_control_objective(ControlObjectiveLine)
where line is a dual quaternion corresponding to a linepassing through the origin of the end-effector frame
Furthermore since any subclass ofDQ_KinematicController provides the methods usedin Listing 4 changing the controller is a matter of changingjust one line of code namely the first one in Listing 4
VI INTERFACE WITH ROSThe Robot Operating System (ROS) [9] is widely used
by the robotics community After the one-line installationdescribed in Section VIII the Python version of the DQRobotics library can be imported in any Python script in thesystem therefore it is compatible with ROS out-of-the-boxMATLAB also has an interface with ROS7
For C++ code in its most recent versions ROS uses thecatkin_build8 environment which itself depends on CMAKEGiven that DQ Robotics C++ is installed as a system library inUbuntu as shown in Section VIII ROS users can readily haveaccess to the library as they would to any other system librarywith a proper CMAKE configuration After installation theC++ version of the library can be linked with
7httpswwwmathworkscomhelprosugget-started-with-roshtml8httpscatkin-toolsreadthedocsioenlatestverbscatkin_buildhtml
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 7
TABLE IIIMAIN METHODS OF CLASS DQ_KinematicController AND ITS MAIN SUBCLASSES
DQ_KinematicControllercompute_setpoint_control_signal Compute the control input to regulate to a set-pointcompute_tracking_control_signal Compute the control input to track a trajectoryget_control_objective Return the control objectiveget_jacobian Return the correct Jacobian based on the control objectiveget_task_variable Return the task variable based on the control objectiveis_set Verify if the controller is set and ready to be usedset_control_objective Set the control objective using predefined goals in ControlObjectiveset_damping Set the damping to prevent instabilities near singular configurationsset_gain Set the controller gainset_primitive_to_effector Attach primitive (eg plane line point) to the end-effectorset_stability_threshold Set the threshold that determines if a stable region has been reachedsystem_reached_stable_region Return true if the trajectories of the closed-loop system have reached a stable region (ie a
positive invariant set) false otherwiseverify_stability Verify if the closed-loop region has reached a stable region
DQ_PseudoinverseSetpointControllercompute_tracking_control_signal Given the task error x = xminus xd isin Rn and the feedforward term xd compute the control
signal u = J+ (minusλx+ xd) where J+ is the pseudoinverse of the task Jacobian J and λis the controller gain
DQ_KinematicConstrainedControllerset_equality_constraint Add equality constraints of type Bq = b where q isin Rn is the vector of joint velocities
b isin Rm is the vector of equality constraints and B isin Rmtimesnset_inequality_constraint Add inequality constraints of type Aq a where q isin Rn is the vector of joint velocities
a isin Rm is the vector of inequality constraints and A isin Rmtimesn
DQ_TaskspaceQuadraticProgrammingControllercompute_objective_function_linear_component Compute the vector h used in the objective function (12)qTHq + hT qcompute_objective_function_symmetric_matrix Compute the matrix H used in the objective function (12)qTHq + hT qcompute_tracking_control_signal Given the task error x = xminus xd isin Rn compute the control signal given by
u isin argminq(12)qTHq + hT q subject to Aq a and Bq = b
DQ_ClassicQPControllercompute_objective_function_linear_component Compute the vector h used in (12)qTHq + hT q = Jq + λxminus xd22compute_objective_function_symmetric_matrix Compute the matrix H used in (12)qTHq + hT q = Jq + λxminus xd22
Abstract methods are written in italics and concrete methods are written in upright In all concrete classes the methodcompute_setpoint_control_signal() does not include the feedforward term in the method parameters and is equivalent to the methodcompute_tracking_control_signal with xd = 0 Therefore they are omitted for the sake of conciseness
target_link_libraries(my_binary dqrobotics)
for a given binary called my_binary
VII INTERFACE WITH V-REPDQ Robotics provides a simple interface to V-REP [10]
enabling users to develop complex simulations without havingto delve into the V-REP documentation The V-REP interfacecomes bundled with the MATLAB and Python versions of thelibrary The C++ version of our V-REP interface comes asa separate package and the following CMAKE directive willlink the required shared objecttarget_link_libraries(my_binary
dqroboticsdqrobotics -vrep-interface)
for a given my_binaryThe available methods are shown in Table IV
To start the communication with V-REP using its remoteAPI9 only two methods are necessaryvi = DQ_VrepInterfaceviconnect(rsquo127001rsquo19997)vistart_simulation()
9httpwwwcoppeliaroboticscomhelpFilesenlegacyRemoteApiOverviewhtm
TABLE IVVrepInterface
connect Connect to a V-REP Remote API Serverdisconnect Disconnect from currently connected serverdisconnect_all Flush all Remote API connectionsstart_simulation Start V-REP simulationstop_simulation Stop V-REP simulationget_object_translation Get object translation as a pure quaternionset_object_translation Set object translation with a pure quaternionget_object_rotation Get object rotation as a unit quaternionset_object_rotation Set object rotation with a unit quaternionget_object_pose Get object pose as a unit dual quaternionset_object_pose Set object pose with a unit dual quaternionset_joint_positions Set the joint positions of a manipulator robotset_joint_target_positions Set the joint target positions of a manipulator
robotget_joint_positions Get the joint positions of a manipulator robot
The method start_simulation starts the V-REP simulationwith the default asynchronous mode and the recommended 5ms communication thread cycle10
Since each robot joint in V-REP is associated with a nameit is convenient to encapsulate this information inside the robotclass Therefore classes implementing robots that use the V-
10For other parameters refer to the code
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 8
REP interface must be a subclass of DQ_VrepRobot to pro-vide the methods send_q_to_vrep and get_q_from_vrepwhich are used to send the desired robot configuration vectorto V-REP and get the current robot configuration vector fromV-REP respectively
Objects poses in a V-REP scene can be retrievedor set by using the methods get_object_pose andset_object_pose respectively which are useful when de-signing motion planners or controllers that take into accountthe constraints imposed by obstacles Finally in order to endthe V-REP simulation it suffices to use two methodsvistop_simulation()vidisconnect()
A A more complete example
To better highlight how DQ Robotics can be used withV-REP in this example a KUKA LWR4 manipulator robotinteracts with a KUKA YouBot mobile manipulator in aworkspace containing three obstacles namely two cylindersand a plane as shown in Fig 8
Fig 8 A more complete example The reference frame is given by F thetime-varying frame Fc is used to indicate where the circle must be drawn inthe whiteboard and Fmm is the end-effector frame of the mobile manipulatorObstacles are represented as geometrical primitives the wall is representedby the plane π = nπ + ε〈pπ nπ〉 where pπ is an arbitrary point onthe plane and nπ is the plane normal and the two cylindrical obstaclesare represented by (l1 R1) and (l2 R2) where li = li + εpi times li isthe ith cylinder centerline and Ri is the corresponding cylinder radius withi isin 1 2 Furthermore li is the line direction and pi is an arbitrary pointon the ith line To prevent collisions between the robot and the obstacles therobot is represented by a circle given by (p3 R3) where p3 and R3 are thecenter and radius of the circle respectively
The manipulator robot holds a whiteboard and uses apseudo-inverse-based controller with a feedforward term totrack a trajectory given by xm(t) = rm(t)xm(0)pm(t)where rm = cos(φ(t)2) + k sin(φ(t)2) with φ(t) =(π2) sin(ωnt) and p = 1 + (12)εdz cos(ωdt)k Viewedfrom the top the whiteboard will follow a semi-circular pathwith a radial oscillation at a frequency of ωd rads andamplitude dzm and an oscillation around the vertical axisat a frequency of ωn rads The mobile manipulator holds afelt pen and follows the manipulator trajectory while drawing a
circle on the whiteboard The mobile manipulator desired end-effector trajectory is given by xmm(t) = xm(t)xc wherexc = 1 + (12)ε0015k is a constant displacement of 15cmalong the z-axis to account for the whiteboard width and = cos(π2) + sin(π2) is a rotation of π around they-axis so that both end-effectors have their z-axis pointingto opposite directions To prevent collision with obstaclesit uses a constrained controller in which for each obstaclewe use a differential inequality to ensure that the robot willapproach it in the worst case with an exponential velocitydecrease For each obstacle we define a distance metricd (t) d (t)minus dsafe where dsafe isin [0infin) is an arbitraryconstant safe distance and the following inequalities must holdfor all t
˙d (t) ge minusηdd (t)lArrrArr minusJdq le ηdd (t) (2)
where ηd isin [0infin) is used to adjust the approach velocity andJd is the distance Jacobian related to the obstacle [11] Thelower is ηd the lower is the allowed approach velocity
In the case of our example to prevent collisions betweenthe robot and the wall we let dsafe R3 (see Fig 8) suchthat d (t) = 0 lArrrArr d (t) = R3 and the distance function isdefined as the distance between p3 which is the center of thecircle enclosing the robot and the wall plane π Analogouslyto prevent collisions between the robot and the static cylinderswe use one inequality such as (2) for each cylinder wheredsafe Ri + R3 and d(t) is defined as the distance betweenp3 and the lines li for i isin 1 2 Although those distancefunctions and corresponding Jacobians can be computed byusing the classes DQ_Geometry and DQ_Kinematics respec-tively details of how they are obtained using dual quaternionalgebra are presented in [11]
Whenever the felt-tip is sufficiently close to the whiteboardit starts to draw a circle Figs 9a and 9b show that the mobilemanipulator successfully draws the circle although there aresome small imperfections as the obstacle avoidance is a hardconstraint that is always respected at the expense of worsetrajectory tracking Last Fig 9c shows the mobile manipulatortrajectory which is always adapted to prevent collisions withthe cylinders and the wall
The main MATLAB code is shown in Listing 5 For the sakeof conciseness the functions compute_lwr4_referenceand compute_youbot_reference which calculate theaforementioned end-effector trajectories are omittedThe same applies for get_plane_from_vrep andget_line_from_vrep which get information about thelocation of the corresponding geometric primitives inV-REP and compute_constraints which calculatesconstraints such as (2) for each obstacle in the scene Theoriginal source code used in this example is available assupplementary material For a more up-to-date version referto httpsgithubcomdqroboticsmatlab-examples
VIII DEVELOPMENT INFRASTRUCTURE
Aiming at the scalability of the DQ Robotics library andtaking advantage of the familiarity that current developers havewith Git and Github we opted to also use those services
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 9
(a) Circle partially drawn at t = 36s
(b) Circle completely drawn at t = 170s
(c) Top view showing the mobile manipulator trajectory
Fig 9 Manipulator robot interacting with a mobile manipulator in aworkspace with two cylindrical obstacles and a wall The parameters for thetrajectory generation were ωn = 01 ωd = 05 and dz = 01
End-users are agnostic to these infrastructural decisions Forthem usage performance and installation of the library matterthe most The usage of the library has been simplified byrelying on a programming syntax similar to the mathemati-
Listing 5 Main MATLAB code for the simulationsampling_time = 005total_time = 200for t=0sampling_timetotal_time
Get obstacles from V-REPplane = get_plane_from_vrep(virsquoObstaclePlanersquoDQk)cylinder1 = get_line_from_vrep(virsquoObstacleCylinder1rsquoDQk)cylinder2 = get_line_from_vrep(virsquoObstacleCylinder2rsquoDQk)
Set references for both robots[lwr4_xd lwr4_ff] = compute_lwr4_reference(lwr4
simulation_parameters lwr4_x0 t)[youbot_xd youbot_ff] = compute_youbot_reference(
youbot_control lwr4_xd lwr4_ff)
Compute the control input for the manipulatorlwr4_u = lwr4_controllercompute_tracking_control_signal(
lwr4_q vec8(lwr4_xd)vec8(lwr4_ff))
Compute constrained control input for the youbot[Jconstraint bconstraint] = compute_constraints(youbot
youbot_q planecylinder1 cylinder2)youbot_controlset_inequality_constraint(-Jconstraint
1bconstraint)youbot_u= youbot_controlcompute_tracking_control_signal(
youbot_q vec8(youbot_xd) vec8(youbot_ff))
Since we are using V-REP just for visualization integrate the control signal to update the robots configurationslwr4_q = lwr4_q + sampling_timelwr4_uyoubot_q = youbot_q + sampling_timeyoubot_u
Send desired values to V-REPlwr4_vreprobotsend_q_to_vrep(lwr4_q)youbot_vreprobotsend_q_to_vrep(youbot_q)end
cal description and being attentive to proper object-orientedprogramming practices Furthermore good performance isachieved by careful optimization of the library and by theC++ implementation
The ease of installation has been addressed in language-platform specific ways that we document in details on the DQRoboticsrsquo Read the Docs11
For MATLAB DQ Robotics is distributed as a Toolboxthat can be installed by the end-user in any of the MATLAB-compatible operating systems
For C++ we officially focus on Ubuntu long-term service(LTS) distributions in a similar way to the most recentdistributions of ROS and other large open-source libraries suchas TensorFlow12 For any Ubuntu LTS distribution that has notreached its end-of-life13 the user can install the C++ versionof the library with the following three commandssudo add-apt-repository ppadqrobotics -devreleasesudo apt-get updatesudo apt-get install libdqrobotics
made available via our release Personal Package Archive(PPA)14 in which we store the latest stable versions of thelibrary Interfaces between DQ Robotics and other librariesalso reside in the same PPA For example after addingthe PPA the user can install the V-REP interface with thefollowing commandsudo apt-get install libdqrobotics -interface -vrep
11httpsdqroboticsgithubioreadthedocsioenlatestinstallationhtml12httpswwwtensorfloworg13httpsubuntucomaboutrelease-cycle14httpslaunchpadnet~dqrobotics-dev+archiveubunturelease
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 10
The PPA guarantees that the user can reliably install thepackage in their system as the PPA only stores packages thatcompiled successfully Support for other operating systemswill be driven by user interest but should not be a bigchallenge for most systems since we are using only CMAKEand Eigen3 as project dependencies both of which are widelysupported
Lastly the Python version of DQ Robotics is made avail-able through a Pybind11-based15 Python wrapper of the C++library This has three main advantages First there is no needto redevelop and maintain versions of the libraries in differentprogramming languages which is highly demanding Secondunit-testing code written in Python which is easier to writeand maintain automatically validates the C++ library as wellThird we can have Pythonrsquos ease-of-use with C++rsquos perfor-mance for each function Although there is a computationaloverhead when using the Python bindings (in contrast withdirectly using the C++ code) it is much lower than a nativePython implementation A simple example of this can be seenin Table V in which we compare the average required timefor dual quaternion multiplications in the same machine
TABLE VCOMPUTATIONAL TIME OF DUAL-QUATERNION MULTIPLICATIONS
Mean [micros] Standard Deviation [micros]C++ (gcc 540) 022 00149
MATLAB R2019a 894 01223Python 352 Bindings 082 00242Python 352 Native 3142 01646
Mean and standard deviation of the required time for one dual-quaternion multiplicationThis was calculated from a thousand sets of a thousand executions on the same IntelCore i9 9900K Ubuntu 1604 x64 system and excludes the time required to generate therandom dual quaternions
An Ubuntu LTS user can install the Python3 version of DQRobotics from the Python Package Index (PyPI)16 with a singleline of code
python3 -m pip install --user dqrobotics
Lastly the PyPI package is continuously built from sourceusing TravisCI17 so that the code is properly compiled andtested before being distributed to our users
IX APPLICATIONS AND COMPARISON WITH OTHERPACKAGES AND LIBRARIES
DQ Robotics has been used for more than nine years tomodel and control different robots in applications such asbimanual surgical robots in constrained workspaces (Fig 10a)whole-body control of humanoid robots (Fig 10b) andthe decentralized formation control of mobile manipulators(Fig 10c)18
The main difference from other packages and librariesis that DQ Robotics implements dual quaternion algebra in
15httpsgithubcompybindpybind1116httpspypiorg17httpstravis-cicom18A more complete list of papers and applications can be found in https
dqroboticsgithubiocitationshtml
(a) The bimanual surgical robot SmartArm [12]
(b) Whole-body control of a humanoid robot [13]
(c) Cooperative mobile manipulators [14]
Fig 10 Examples of different applications of DQ Robotics
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 11
a close way to the mathematical notation Furthermore allsupported languages namely Python MATLAB and C++use the same convention and style and it is straightforwardto port code in one language to other supported languages Toenable that we avoid using features that are not available in allthree languages unless that would lead to poor-quality codeFrom the user point-of-view the advantage of such approachis that users can prototype their ideas on MATLAB usingthe convenient plot system then easily translate the code toC++ and deploy it on a real robotic platform The Pythonversion of the library is somewhere between MATLAB andC++ being suitable for prototyping and also for deploymenton real applications because C++ runs under the hood
In terms of target audience the MATLAB version of DQRobotics has some overlap with Peter Corkersquos Robotics Tool-box [15] most notably in terms of their use for educationand learning On the one hand earlier versions were greatlyinspired by Peter Corkersquos Robotics Toolbox most notablythe plot system which is particularly useful when teachingrobotics On the other hand DQ Robotics is entirely basedon general dual quaternion algebra whereas Robotics Tool-box uses classic representations for robot modeling althoughit does offer support for unit quaternions Moreover otherfunctionalities of Peter Corkersquos Robotics Toolbox such aspath planning localization and mapping are not available inDQ Robotics as of now However some functionalities ofPeter Corkersquos Robotics Toolbox are now available in toolboxesdistributed by MathWorks such as the Image ProcessingToolbox19 and the Robotics System Toolbox20 which are inprinciple compatible with DQ Robotics
Another MATLAB toolbox [7] focuses on the application ofunit dual quaternions to the neuroscience domain and presentsbasic kinematic modeling of serial mechanisms In addition tohaving many fewer functionalities than DQ Robotics it usesprocedural programming which makes the code quite differentfrom the notation used on paper
A major contrast between DQ Robotics and Peter Corkersquosand Leclercqrsquos MATLAB toolboxes is that DQ Robotics hasPython and C++ versions with identical APIs as far as theprogramming languages permit This increases the numberof possible users and considerably reduces the time requiredto go from prototyping to deployment Moreover this makesDQ Robotics compatible with powerful libraries available inPython such as SciPy21 and scikit-learn22 In addition bothPython and C++ versions of the library can be readily usedalongside ROS which is the current standard of shareablecode in the robotics community Lastly a modern developmentinfrastructure not available in other existing libraries makesthe library scalable and easier to maintain and extend
X CONCLUSIONS
This paper presented DQ Robotics a comprehensive com-putational library for robot modeling and control using dual
19httpswwwmathworkscomproductsimagehtml20httpswwwmathworkscomproductsroboticshtml21httpswwwscipyorg22httpsscikit-learnorgstable
quaternion algebra It has the unique feature of using a notationvery close to the mathematical description and supportingthree different programming languages namely MATLABPython and C++ while using very similar conventions amongthem making it very easy to switch between languages andthus shortening the development time from prototyping tothe implementation on actual robots The library currentlysupports serial manipulator robots cooperative systems suchas two-arm robots mobile manipulators and even branchedmechanisms such as humanoids Robot models are generatedautomatically from simple geometrical parameters such asthe Denavit-Hartenberg parameters In addition robots canbe easily combined to yield more complex ones while thecorresponding forward kinematics and differential kinematicsare also computed automatically and analytically at executiontime The library also offers a rich set of motion controllersand it is very simple to implement new ones Although it doesnot offer dynamics modeling yet it can be easily integrated toexisting libraries for that purpose However robot dynamicsmodeling using dual quaternion algebra is currently underdevelopment and will be integrated into the library as soonas it becomes mature Rather than being a competitor toexisting libraries DQ Robotics aims at complementing themand helping in the popularization of dual quaternion algebrain the robotics domain
ACKNOWLEDGMENT
The authors would like to thank all users of the DQ Roboticslibrary for their valuable feedback and bug reports speciallyJuan Joseacute Quiroz-Omantildea and other members of the MACROresearch group at UFMG
REFERENCES
[1] O Bottema and B Roth Theoretical kinematics North-HollandPublishing Company 1979 vol 24
[2] J McCarthy Introduction to theoretical kinematics 1st ed The MITPress 1990
[3] B V Adorno ldquoTwo-arm Manipulation From Manipulators to EnhancedHuman-Robot Collaboration [Contribution agrave la manipulation agrave deuxbras des manipulateurs agrave la collaboration homme-robot]rdquo PhDDissertation Universiteacute Montpellier 2 2011 [Online] Availablehttpstelarchives-ouvertesfrtel-00641678
[4] M W Spong and M Vidyasagar Robot dynamics and control JohnWiley amp Sons 2008
[5] B Siciliano L Sciavicco L Villani and G Oriolo Robotics modellingplanning and control Springer Science amp Business Media 2010
[6] B Siciliano and O Khatib Springer handbook of robotics Springer2016
[7] G Leclercq P Lefegravevre and G Blohm ldquo3D kinematics using dualquaternions theory and applications in neurosciencerdquo Frontiers inBehavioral Neuroscience vol 7 no February p 7 jan 2013 [Online]Available httpwwwncbinlmnihgovpubmed23443667httpjournalfrontiersinorgarticle103389fnbeh201300007abstract
[8] B V Adorno ldquoRobot Kinematic Modeling and Control Based on DualQuaternion Algebra ndash Part I Fundamentalsrdquo 2017 [Online] Availablehttpshalarchives-ouvertesfrhal-01478225v1
[9] M Quigley K Conley B Gerkey J Faust T Foote J LeibsR Wheeler and A Y Ng ldquoRos an open-source robot operating systemrdquoin ICRA workshop on open source software vol 3 no 32 Kobe Japan2009 p 5
[10] E Rohmer S P N Singh and M Freese ldquoV-REP A versatile andscalable robot simulation frameworkrdquo in 2013 IEEERSJ InternationalConference on Intelligent Robots and Systems IEEE nov 2013
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 12
[11] M M Marinho B V Adorno K Harada and M Mitsuishi ldquoDynamicActive Constraints for Surgical Robots Using Vector-Field InequalitiesrdquoIEEE Transactions on Robotics vol 35 no 5 pp 1166ndash1185 oct 2019[Online] Available httpsieeexploreieeeorgdocument8742769
[12] M M Marinho K Harada A Morita and M Mitsuishi ldquoSmartarmIntegration and validation of a versatile surgical robotic system forconstrained workspacesrdquo The International Journal of Medical Roboticsand Computer Assisted Surgery (in press) 2019
[13] J J Quiroz-Omana and B V Adorno ldquoWhole-Body Control With(Self) Collision Avoidance Using Vector Field Inequalitiesrdquo IEEERobotics and Automation Letters vol 4 no 4 pp 4048ndash4053 oct 2019
[Online] Available httpsieeexploreieeeorgdocument8763977[14] H J Savino L C Pimenta J A Shah and B V Adorno
ldquoPose consensus based on dual quaternion algebra with applicationto decentralized formation control of mobile manipulatorsrdquo Journalof the Franklin Institute pp 1ndash36 oct 2019 [Online] AvailablehttpslinkinghubelseviercomretrievepiiS0016003219307161
[15] P Corke Robotics Vision and Control 2nd ed ser SpringerTracts in Advanced Robotics Springer International Publishing2017 vol 118 [Online] Available httplinkspringercom101007978-3-319-54413-7
- I Introduction
-
- I-A How to follow the tutorial
- I-B Notation choice and common operations
-
- II Representing rigid motions
- III Robot kinematics
- IV Geometric Primitives
- V Robot motion control
- VI Interface with ROS
- VII Interface with V-REP
-
- VII-A A more complete example
-
- VIII Development infrastructure
- IX Applications and comparison with other packages and libraries
- X Conclusions
- References
-
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 7
TABLE IIIMAIN METHODS OF CLASS DQ_KinematicController AND ITS MAIN SUBCLASSES
DQ_KinematicControllercompute_setpoint_control_signal Compute the control input to regulate to a set-pointcompute_tracking_control_signal Compute the control input to track a trajectoryget_control_objective Return the control objectiveget_jacobian Return the correct Jacobian based on the control objectiveget_task_variable Return the task variable based on the control objectiveis_set Verify if the controller is set and ready to be usedset_control_objective Set the control objective using predefined goals in ControlObjectiveset_damping Set the damping to prevent instabilities near singular configurationsset_gain Set the controller gainset_primitive_to_effector Attach primitive (eg plane line point) to the end-effectorset_stability_threshold Set the threshold that determines if a stable region has been reachedsystem_reached_stable_region Return true if the trajectories of the closed-loop system have reached a stable region (ie a
positive invariant set) false otherwiseverify_stability Verify if the closed-loop region has reached a stable region
DQ_PseudoinverseSetpointControllercompute_tracking_control_signal Given the task error x = xminus xd isin Rn and the feedforward term xd compute the control
signal u = J+ (minusλx+ xd) where J+ is the pseudoinverse of the task Jacobian J and λis the controller gain
DQ_KinematicConstrainedControllerset_equality_constraint Add equality constraints of type Bq = b where q isin Rn is the vector of joint velocities
b isin Rm is the vector of equality constraints and B isin Rmtimesnset_inequality_constraint Add inequality constraints of type Aq a where q isin Rn is the vector of joint velocities
a isin Rm is the vector of inequality constraints and A isin Rmtimesn
DQ_TaskspaceQuadraticProgrammingControllercompute_objective_function_linear_component Compute the vector h used in the objective function (12)qTHq + hT qcompute_objective_function_symmetric_matrix Compute the matrix H used in the objective function (12)qTHq + hT qcompute_tracking_control_signal Given the task error x = xminus xd isin Rn compute the control signal given by
u isin argminq(12)qTHq + hT q subject to Aq a and Bq = b
DQ_ClassicQPControllercompute_objective_function_linear_component Compute the vector h used in (12)qTHq + hT q = Jq + λxminus xd22compute_objective_function_symmetric_matrix Compute the matrix H used in (12)qTHq + hT q = Jq + λxminus xd22
Abstract methods are written in italics and concrete methods are written in upright In all concrete classes the methodcompute_setpoint_control_signal() does not include the feedforward term in the method parameters and is equivalent to the methodcompute_tracking_control_signal with xd = 0 Therefore they are omitted for the sake of conciseness
target_link_libraries(my_binary dqrobotics)
for a given binary called my_binary
VII INTERFACE WITH V-REPDQ Robotics provides a simple interface to V-REP [10]
enabling users to develop complex simulations without havingto delve into the V-REP documentation The V-REP interfacecomes bundled with the MATLAB and Python versions of thelibrary The C++ version of our V-REP interface comes asa separate package and the following CMAKE directive willlink the required shared objecttarget_link_libraries(my_binary
dqroboticsdqrobotics -vrep-interface)
for a given my_binaryThe available methods are shown in Table IV
To start the communication with V-REP using its remoteAPI9 only two methods are necessaryvi = DQ_VrepInterfaceviconnect(rsquo127001rsquo19997)vistart_simulation()
9httpwwwcoppeliaroboticscomhelpFilesenlegacyRemoteApiOverviewhtm
TABLE IVVrepInterface
connect Connect to a V-REP Remote API Serverdisconnect Disconnect from currently connected serverdisconnect_all Flush all Remote API connectionsstart_simulation Start V-REP simulationstop_simulation Stop V-REP simulationget_object_translation Get object translation as a pure quaternionset_object_translation Set object translation with a pure quaternionget_object_rotation Get object rotation as a unit quaternionset_object_rotation Set object rotation with a unit quaternionget_object_pose Get object pose as a unit dual quaternionset_object_pose Set object pose with a unit dual quaternionset_joint_positions Set the joint positions of a manipulator robotset_joint_target_positions Set the joint target positions of a manipulator
robotget_joint_positions Get the joint positions of a manipulator robot
The method start_simulation starts the V-REP simulationwith the default asynchronous mode and the recommended 5ms communication thread cycle10
Since each robot joint in V-REP is associated with a nameit is convenient to encapsulate this information inside the robotclass Therefore classes implementing robots that use the V-
10For other parameters refer to the code
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 8
REP interface must be a subclass of DQ_VrepRobot to pro-vide the methods send_q_to_vrep and get_q_from_vrepwhich are used to send the desired robot configuration vectorto V-REP and get the current robot configuration vector fromV-REP respectively
Objects poses in a V-REP scene can be retrievedor set by using the methods get_object_pose andset_object_pose respectively which are useful when de-signing motion planners or controllers that take into accountthe constraints imposed by obstacles Finally in order to endthe V-REP simulation it suffices to use two methodsvistop_simulation()vidisconnect()
A A more complete example
To better highlight how DQ Robotics can be used withV-REP in this example a KUKA LWR4 manipulator robotinteracts with a KUKA YouBot mobile manipulator in aworkspace containing three obstacles namely two cylindersand a plane as shown in Fig 8
Fig 8 A more complete example The reference frame is given by F thetime-varying frame Fc is used to indicate where the circle must be drawn inthe whiteboard and Fmm is the end-effector frame of the mobile manipulatorObstacles are represented as geometrical primitives the wall is representedby the plane π = nπ + ε〈pπ nπ〉 where pπ is an arbitrary point onthe plane and nπ is the plane normal and the two cylindrical obstaclesare represented by (l1 R1) and (l2 R2) where li = li + εpi times li isthe ith cylinder centerline and Ri is the corresponding cylinder radius withi isin 1 2 Furthermore li is the line direction and pi is an arbitrary pointon the ith line To prevent collisions between the robot and the obstacles therobot is represented by a circle given by (p3 R3) where p3 and R3 are thecenter and radius of the circle respectively
The manipulator robot holds a whiteboard and uses apseudo-inverse-based controller with a feedforward term totrack a trajectory given by xm(t) = rm(t)xm(0)pm(t)where rm = cos(φ(t)2) + k sin(φ(t)2) with φ(t) =(π2) sin(ωnt) and p = 1 + (12)εdz cos(ωdt)k Viewedfrom the top the whiteboard will follow a semi-circular pathwith a radial oscillation at a frequency of ωd rads andamplitude dzm and an oscillation around the vertical axisat a frequency of ωn rads The mobile manipulator holds afelt pen and follows the manipulator trajectory while drawing a
circle on the whiteboard The mobile manipulator desired end-effector trajectory is given by xmm(t) = xm(t)xc wherexc = 1 + (12)ε0015k is a constant displacement of 15cmalong the z-axis to account for the whiteboard width and = cos(π2) + sin(π2) is a rotation of π around they-axis so that both end-effectors have their z-axis pointingto opposite directions To prevent collision with obstaclesit uses a constrained controller in which for each obstaclewe use a differential inequality to ensure that the robot willapproach it in the worst case with an exponential velocitydecrease For each obstacle we define a distance metricd (t) d (t)minus dsafe where dsafe isin [0infin) is an arbitraryconstant safe distance and the following inequalities must holdfor all t
˙d (t) ge minusηdd (t)lArrrArr minusJdq le ηdd (t) (2)
where ηd isin [0infin) is used to adjust the approach velocity andJd is the distance Jacobian related to the obstacle [11] Thelower is ηd the lower is the allowed approach velocity
In the case of our example to prevent collisions betweenthe robot and the wall we let dsafe R3 (see Fig 8) suchthat d (t) = 0 lArrrArr d (t) = R3 and the distance function isdefined as the distance between p3 which is the center of thecircle enclosing the robot and the wall plane π Analogouslyto prevent collisions between the robot and the static cylinderswe use one inequality such as (2) for each cylinder wheredsafe Ri + R3 and d(t) is defined as the distance betweenp3 and the lines li for i isin 1 2 Although those distancefunctions and corresponding Jacobians can be computed byusing the classes DQ_Geometry and DQ_Kinematics respec-tively details of how they are obtained using dual quaternionalgebra are presented in [11]
Whenever the felt-tip is sufficiently close to the whiteboardit starts to draw a circle Figs 9a and 9b show that the mobilemanipulator successfully draws the circle although there aresome small imperfections as the obstacle avoidance is a hardconstraint that is always respected at the expense of worsetrajectory tracking Last Fig 9c shows the mobile manipulatortrajectory which is always adapted to prevent collisions withthe cylinders and the wall
The main MATLAB code is shown in Listing 5 For the sakeof conciseness the functions compute_lwr4_referenceand compute_youbot_reference which calculate theaforementioned end-effector trajectories are omittedThe same applies for get_plane_from_vrep andget_line_from_vrep which get information about thelocation of the corresponding geometric primitives inV-REP and compute_constraints which calculatesconstraints such as (2) for each obstacle in the scene Theoriginal source code used in this example is available assupplementary material For a more up-to-date version referto httpsgithubcomdqroboticsmatlab-examples
VIII DEVELOPMENT INFRASTRUCTURE
Aiming at the scalability of the DQ Robotics library andtaking advantage of the familiarity that current developers havewith Git and Github we opted to also use those services
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 9
(a) Circle partially drawn at t = 36s
(b) Circle completely drawn at t = 170s
(c) Top view showing the mobile manipulator trajectory
Fig 9 Manipulator robot interacting with a mobile manipulator in aworkspace with two cylindrical obstacles and a wall The parameters for thetrajectory generation were ωn = 01 ωd = 05 and dz = 01
End-users are agnostic to these infrastructural decisions Forthem usage performance and installation of the library matterthe most The usage of the library has been simplified byrelying on a programming syntax similar to the mathemati-
Listing 5 Main MATLAB code for the simulationsampling_time = 005total_time = 200for t=0sampling_timetotal_time
Get obstacles from V-REPplane = get_plane_from_vrep(virsquoObstaclePlanersquoDQk)cylinder1 = get_line_from_vrep(virsquoObstacleCylinder1rsquoDQk)cylinder2 = get_line_from_vrep(virsquoObstacleCylinder2rsquoDQk)
Set references for both robots[lwr4_xd lwr4_ff] = compute_lwr4_reference(lwr4
simulation_parameters lwr4_x0 t)[youbot_xd youbot_ff] = compute_youbot_reference(
youbot_control lwr4_xd lwr4_ff)
Compute the control input for the manipulatorlwr4_u = lwr4_controllercompute_tracking_control_signal(
lwr4_q vec8(lwr4_xd)vec8(lwr4_ff))
Compute constrained control input for the youbot[Jconstraint bconstraint] = compute_constraints(youbot
youbot_q planecylinder1 cylinder2)youbot_controlset_inequality_constraint(-Jconstraint
1bconstraint)youbot_u= youbot_controlcompute_tracking_control_signal(
youbot_q vec8(youbot_xd) vec8(youbot_ff))
Since we are using V-REP just for visualization integrate the control signal to update the robots configurationslwr4_q = lwr4_q + sampling_timelwr4_uyoubot_q = youbot_q + sampling_timeyoubot_u
Send desired values to V-REPlwr4_vreprobotsend_q_to_vrep(lwr4_q)youbot_vreprobotsend_q_to_vrep(youbot_q)end
cal description and being attentive to proper object-orientedprogramming practices Furthermore good performance isachieved by careful optimization of the library and by theC++ implementation
The ease of installation has been addressed in language-platform specific ways that we document in details on the DQRoboticsrsquo Read the Docs11
For MATLAB DQ Robotics is distributed as a Toolboxthat can be installed by the end-user in any of the MATLAB-compatible operating systems
For C++ we officially focus on Ubuntu long-term service(LTS) distributions in a similar way to the most recentdistributions of ROS and other large open-source libraries suchas TensorFlow12 For any Ubuntu LTS distribution that has notreached its end-of-life13 the user can install the C++ versionof the library with the following three commandssudo add-apt-repository ppadqrobotics -devreleasesudo apt-get updatesudo apt-get install libdqrobotics
made available via our release Personal Package Archive(PPA)14 in which we store the latest stable versions of thelibrary Interfaces between DQ Robotics and other librariesalso reside in the same PPA For example after addingthe PPA the user can install the V-REP interface with thefollowing commandsudo apt-get install libdqrobotics -interface -vrep
11httpsdqroboticsgithubioreadthedocsioenlatestinstallationhtml12httpswwwtensorfloworg13httpsubuntucomaboutrelease-cycle14httpslaunchpadnet~dqrobotics-dev+archiveubunturelease
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 10
The PPA guarantees that the user can reliably install thepackage in their system as the PPA only stores packages thatcompiled successfully Support for other operating systemswill be driven by user interest but should not be a bigchallenge for most systems since we are using only CMAKEand Eigen3 as project dependencies both of which are widelysupported
Lastly the Python version of DQ Robotics is made avail-able through a Pybind11-based15 Python wrapper of the C++library This has three main advantages First there is no needto redevelop and maintain versions of the libraries in differentprogramming languages which is highly demanding Secondunit-testing code written in Python which is easier to writeand maintain automatically validates the C++ library as wellThird we can have Pythonrsquos ease-of-use with C++rsquos perfor-mance for each function Although there is a computationaloverhead when using the Python bindings (in contrast withdirectly using the C++ code) it is much lower than a nativePython implementation A simple example of this can be seenin Table V in which we compare the average required timefor dual quaternion multiplications in the same machine
TABLE VCOMPUTATIONAL TIME OF DUAL-QUATERNION MULTIPLICATIONS
Mean [micros] Standard Deviation [micros]C++ (gcc 540) 022 00149
MATLAB R2019a 894 01223Python 352 Bindings 082 00242Python 352 Native 3142 01646
Mean and standard deviation of the required time for one dual-quaternion multiplicationThis was calculated from a thousand sets of a thousand executions on the same IntelCore i9 9900K Ubuntu 1604 x64 system and excludes the time required to generate therandom dual quaternions
An Ubuntu LTS user can install the Python3 version of DQRobotics from the Python Package Index (PyPI)16 with a singleline of code
python3 -m pip install --user dqrobotics
Lastly the PyPI package is continuously built from sourceusing TravisCI17 so that the code is properly compiled andtested before being distributed to our users
IX APPLICATIONS AND COMPARISON WITH OTHERPACKAGES AND LIBRARIES
DQ Robotics has been used for more than nine years tomodel and control different robots in applications such asbimanual surgical robots in constrained workspaces (Fig 10a)whole-body control of humanoid robots (Fig 10b) andthe decentralized formation control of mobile manipulators(Fig 10c)18
The main difference from other packages and librariesis that DQ Robotics implements dual quaternion algebra in
15httpsgithubcompybindpybind1116httpspypiorg17httpstravis-cicom18A more complete list of papers and applications can be found in https
dqroboticsgithubiocitationshtml
(a) The bimanual surgical robot SmartArm [12]
(b) Whole-body control of a humanoid robot [13]
(c) Cooperative mobile manipulators [14]
Fig 10 Examples of different applications of DQ Robotics
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 11
a close way to the mathematical notation Furthermore allsupported languages namely Python MATLAB and C++use the same convention and style and it is straightforwardto port code in one language to other supported languages Toenable that we avoid using features that are not available in allthree languages unless that would lead to poor-quality codeFrom the user point-of-view the advantage of such approachis that users can prototype their ideas on MATLAB usingthe convenient plot system then easily translate the code toC++ and deploy it on a real robotic platform The Pythonversion of the library is somewhere between MATLAB andC++ being suitable for prototyping and also for deploymenton real applications because C++ runs under the hood
In terms of target audience the MATLAB version of DQRobotics has some overlap with Peter Corkersquos Robotics Tool-box [15] most notably in terms of their use for educationand learning On the one hand earlier versions were greatlyinspired by Peter Corkersquos Robotics Toolbox most notablythe plot system which is particularly useful when teachingrobotics On the other hand DQ Robotics is entirely basedon general dual quaternion algebra whereas Robotics Tool-box uses classic representations for robot modeling althoughit does offer support for unit quaternions Moreover otherfunctionalities of Peter Corkersquos Robotics Toolbox such aspath planning localization and mapping are not available inDQ Robotics as of now However some functionalities ofPeter Corkersquos Robotics Toolbox are now available in toolboxesdistributed by MathWorks such as the Image ProcessingToolbox19 and the Robotics System Toolbox20 which are inprinciple compatible with DQ Robotics
Another MATLAB toolbox [7] focuses on the application ofunit dual quaternions to the neuroscience domain and presentsbasic kinematic modeling of serial mechanisms In addition tohaving many fewer functionalities than DQ Robotics it usesprocedural programming which makes the code quite differentfrom the notation used on paper
A major contrast between DQ Robotics and Peter Corkersquosand Leclercqrsquos MATLAB toolboxes is that DQ Robotics hasPython and C++ versions with identical APIs as far as theprogramming languages permit This increases the numberof possible users and considerably reduces the time requiredto go from prototyping to deployment Moreover this makesDQ Robotics compatible with powerful libraries available inPython such as SciPy21 and scikit-learn22 In addition bothPython and C++ versions of the library can be readily usedalongside ROS which is the current standard of shareablecode in the robotics community Lastly a modern developmentinfrastructure not available in other existing libraries makesthe library scalable and easier to maintain and extend
X CONCLUSIONS
This paper presented DQ Robotics a comprehensive com-putational library for robot modeling and control using dual
19httpswwwmathworkscomproductsimagehtml20httpswwwmathworkscomproductsroboticshtml21httpswwwscipyorg22httpsscikit-learnorgstable
quaternion algebra It has the unique feature of using a notationvery close to the mathematical description and supportingthree different programming languages namely MATLABPython and C++ while using very similar conventions amongthem making it very easy to switch between languages andthus shortening the development time from prototyping tothe implementation on actual robots The library currentlysupports serial manipulator robots cooperative systems suchas two-arm robots mobile manipulators and even branchedmechanisms such as humanoids Robot models are generatedautomatically from simple geometrical parameters such asthe Denavit-Hartenberg parameters In addition robots canbe easily combined to yield more complex ones while thecorresponding forward kinematics and differential kinematicsare also computed automatically and analytically at executiontime The library also offers a rich set of motion controllersand it is very simple to implement new ones Although it doesnot offer dynamics modeling yet it can be easily integrated toexisting libraries for that purpose However robot dynamicsmodeling using dual quaternion algebra is currently underdevelopment and will be integrated into the library as soonas it becomes mature Rather than being a competitor toexisting libraries DQ Robotics aims at complementing themand helping in the popularization of dual quaternion algebrain the robotics domain
ACKNOWLEDGMENT
The authors would like to thank all users of the DQ Roboticslibrary for their valuable feedback and bug reports speciallyJuan Joseacute Quiroz-Omantildea and other members of the MACROresearch group at UFMG
REFERENCES
[1] O Bottema and B Roth Theoretical kinematics North-HollandPublishing Company 1979 vol 24
[2] J McCarthy Introduction to theoretical kinematics 1st ed The MITPress 1990
[3] B V Adorno ldquoTwo-arm Manipulation From Manipulators to EnhancedHuman-Robot Collaboration [Contribution agrave la manipulation agrave deuxbras des manipulateurs agrave la collaboration homme-robot]rdquo PhDDissertation Universiteacute Montpellier 2 2011 [Online] Availablehttpstelarchives-ouvertesfrtel-00641678
[4] M W Spong and M Vidyasagar Robot dynamics and control JohnWiley amp Sons 2008
[5] B Siciliano L Sciavicco L Villani and G Oriolo Robotics modellingplanning and control Springer Science amp Business Media 2010
[6] B Siciliano and O Khatib Springer handbook of robotics Springer2016
[7] G Leclercq P Lefegravevre and G Blohm ldquo3D kinematics using dualquaternions theory and applications in neurosciencerdquo Frontiers inBehavioral Neuroscience vol 7 no February p 7 jan 2013 [Online]Available httpwwwncbinlmnihgovpubmed23443667httpjournalfrontiersinorgarticle103389fnbeh201300007abstract
[8] B V Adorno ldquoRobot Kinematic Modeling and Control Based on DualQuaternion Algebra ndash Part I Fundamentalsrdquo 2017 [Online] Availablehttpshalarchives-ouvertesfrhal-01478225v1
[9] M Quigley K Conley B Gerkey J Faust T Foote J LeibsR Wheeler and A Y Ng ldquoRos an open-source robot operating systemrdquoin ICRA workshop on open source software vol 3 no 32 Kobe Japan2009 p 5
[10] E Rohmer S P N Singh and M Freese ldquoV-REP A versatile andscalable robot simulation frameworkrdquo in 2013 IEEERSJ InternationalConference on Intelligent Robots and Systems IEEE nov 2013
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 12
[11] M M Marinho B V Adorno K Harada and M Mitsuishi ldquoDynamicActive Constraints for Surgical Robots Using Vector-Field InequalitiesrdquoIEEE Transactions on Robotics vol 35 no 5 pp 1166ndash1185 oct 2019[Online] Available httpsieeexploreieeeorgdocument8742769
[12] M M Marinho K Harada A Morita and M Mitsuishi ldquoSmartarmIntegration and validation of a versatile surgical robotic system forconstrained workspacesrdquo The International Journal of Medical Roboticsand Computer Assisted Surgery (in press) 2019
[13] J J Quiroz-Omana and B V Adorno ldquoWhole-Body Control With(Self) Collision Avoidance Using Vector Field Inequalitiesrdquo IEEERobotics and Automation Letters vol 4 no 4 pp 4048ndash4053 oct 2019
[Online] Available httpsieeexploreieeeorgdocument8763977[14] H J Savino L C Pimenta J A Shah and B V Adorno
ldquoPose consensus based on dual quaternion algebra with applicationto decentralized formation control of mobile manipulatorsrdquo Journalof the Franklin Institute pp 1ndash36 oct 2019 [Online] AvailablehttpslinkinghubelseviercomretrievepiiS0016003219307161
[15] P Corke Robotics Vision and Control 2nd ed ser SpringerTracts in Advanced Robotics Springer International Publishing2017 vol 118 [Online] Available httplinkspringercom101007978-3-319-54413-7
- I Introduction
-
- I-A How to follow the tutorial
- I-B Notation choice and common operations
-
- II Representing rigid motions
- III Robot kinematics
- IV Geometric Primitives
- V Robot motion control
- VI Interface with ROS
- VII Interface with V-REP
-
- VII-A A more complete example
-
- VIII Development infrastructure
- IX Applications and comparison with other packages and libraries
- X Conclusions
- References
-
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 8
REP interface must be a subclass of DQ_VrepRobot to pro-vide the methods send_q_to_vrep and get_q_from_vrepwhich are used to send the desired robot configuration vectorto V-REP and get the current robot configuration vector fromV-REP respectively
Objects poses in a V-REP scene can be retrievedor set by using the methods get_object_pose andset_object_pose respectively which are useful when de-signing motion planners or controllers that take into accountthe constraints imposed by obstacles Finally in order to endthe V-REP simulation it suffices to use two methodsvistop_simulation()vidisconnect()
A A more complete example
To better highlight how DQ Robotics can be used withV-REP in this example a KUKA LWR4 manipulator robotinteracts with a KUKA YouBot mobile manipulator in aworkspace containing three obstacles namely two cylindersand a plane as shown in Fig 8
Fig 8 A more complete example The reference frame is given by F thetime-varying frame Fc is used to indicate where the circle must be drawn inthe whiteboard and Fmm is the end-effector frame of the mobile manipulatorObstacles are represented as geometrical primitives the wall is representedby the plane π = nπ + ε〈pπ nπ〉 where pπ is an arbitrary point onthe plane and nπ is the plane normal and the two cylindrical obstaclesare represented by (l1 R1) and (l2 R2) where li = li + εpi times li isthe ith cylinder centerline and Ri is the corresponding cylinder radius withi isin 1 2 Furthermore li is the line direction and pi is an arbitrary pointon the ith line To prevent collisions between the robot and the obstacles therobot is represented by a circle given by (p3 R3) where p3 and R3 are thecenter and radius of the circle respectively
The manipulator robot holds a whiteboard and uses apseudo-inverse-based controller with a feedforward term totrack a trajectory given by xm(t) = rm(t)xm(0)pm(t)where rm = cos(φ(t)2) + k sin(φ(t)2) with φ(t) =(π2) sin(ωnt) and p = 1 + (12)εdz cos(ωdt)k Viewedfrom the top the whiteboard will follow a semi-circular pathwith a radial oscillation at a frequency of ωd rads andamplitude dzm and an oscillation around the vertical axisat a frequency of ωn rads The mobile manipulator holds afelt pen and follows the manipulator trajectory while drawing a
circle on the whiteboard The mobile manipulator desired end-effector trajectory is given by xmm(t) = xm(t)xc wherexc = 1 + (12)ε0015k is a constant displacement of 15cmalong the z-axis to account for the whiteboard width and = cos(π2) + sin(π2) is a rotation of π around they-axis so that both end-effectors have their z-axis pointingto opposite directions To prevent collision with obstaclesit uses a constrained controller in which for each obstaclewe use a differential inequality to ensure that the robot willapproach it in the worst case with an exponential velocitydecrease For each obstacle we define a distance metricd (t) d (t)minus dsafe where dsafe isin [0infin) is an arbitraryconstant safe distance and the following inequalities must holdfor all t
˙d (t) ge minusηdd (t)lArrrArr minusJdq le ηdd (t) (2)
where ηd isin [0infin) is used to adjust the approach velocity andJd is the distance Jacobian related to the obstacle [11] Thelower is ηd the lower is the allowed approach velocity
In the case of our example to prevent collisions betweenthe robot and the wall we let dsafe R3 (see Fig 8) suchthat d (t) = 0 lArrrArr d (t) = R3 and the distance function isdefined as the distance between p3 which is the center of thecircle enclosing the robot and the wall plane π Analogouslyto prevent collisions between the robot and the static cylinderswe use one inequality such as (2) for each cylinder wheredsafe Ri + R3 and d(t) is defined as the distance betweenp3 and the lines li for i isin 1 2 Although those distancefunctions and corresponding Jacobians can be computed byusing the classes DQ_Geometry and DQ_Kinematics respec-tively details of how they are obtained using dual quaternionalgebra are presented in [11]
Whenever the felt-tip is sufficiently close to the whiteboardit starts to draw a circle Figs 9a and 9b show that the mobilemanipulator successfully draws the circle although there aresome small imperfections as the obstacle avoidance is a hardconstraint that is always respected at the expense of worsetrajectory tracking Last Fig 9c shows the mobile manipulatortrajectory which is always adapted to prevent collisions withthe cylinders and the wall
The main MATLAB code is shown in Listing 5 For the sakeof conciseness the functions compute_lwr4_referenceand compute_youbot_reference which calculate theaforementioned end-effector trajectories are omittedThe same applies for get_plane_from_vrep andget_line_from_vrep which get information about thelocation of the corresponding geometric primitives inV-REP and compute_constraints which calculatesconstraints such as (2) for each obstacle in the scene Theoriginal source code used in this example is available assupplementary material For a more up-to-date version referto httpsgithubcomdqroboticsmatlab-examples
VIII DEVELOPMENT INFRASTRUCTURE
Aiming at the scalability of the DQ Robotics library andtaking advantage of the familiarity that current developers havewith Git and Github we opted to also use those services
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 9
(a) Circle partially drawn at t = 36s
(b) Circle completely drawn at t = 170s
(c) Top view showing the mobile manipulator trajectory
Fig 9 Manipulator robot interacting with a mobile manipulator in aworkspace with two cylindrical obstacles and a wall The parameters for thetrajectory generation were ωn = 01 ωd = 05 and dz = 01
End-users are agnostic to these infrastructural decisions Forthem usage performance and installation of the library matterthe most The usage of the library has been simplified byrelying on a programming syntax similar to the mathemati-
Listing 5 Main MATLAB code for the simulationsampling_time = 005total_time = 200for t=0sampling_timetotal_time
Get obstacles from V-REPplane = get_plane_from_vrep(virsquoObstaclePlanersquoDQk)cylinder1 = get_line_from_vrep(virsquoObstacleCylinder1rsquoDQk)cylinder2 = get_line_from_vrep(virsquoObstacleCylinder2rsquoDQk)
Set references for both robots[lwr4_xd lwr4_ff] = compute_lwr4_reference(lwr4
simulation_parameters lwr4_x0 t)[youbot_xd youbot_ff] = compute_youbot_reference(
youbot_control lwr4_xd lwr4_ff)
Compute the control input for the manipulatorlwr4_u = lwr4_controllercompute_tracking_control_signal(
lwr4_q vec8(lwr4_xd)vec8(lwr4_ff))
Compute constrained control input for the youbot[Jconstraint bconstraint] = compute_constraints(youbot
youbot_q planecylinder1 cylinder2)youbot_controlset_inequality_constraint(-Jconstraint
1bconstraint)youbot_u= youbot_controlcompute_tracking_control_signal(
youbot_q vec8(youbot_xd) vec8(youbot_ff))
Since we are using V-REP just for visualization integrate the control signal to update the robots configurationslwr4_q = lwr4_q + sampling_timelwr4_uyoubot_q = youbot_q + sampling_timeyoubot_u
Send desired values to V-REPlwr4_vreprobotsend_q_to_vrep(lwr4_q)youbot_vreprobotsend_q_to_vrep(youbot_q)end
cal description and being attentive to proper object-orientedprogramming practices Furthermore good performance isachieved by careful optimization of the library and by theC++ implementation
The ease of installation has been addressed in language-platform specific ways that we document in details on the DQRoboticsrsquo Read the Docs11
For MATLAB DQ Robotics is distributed as a Toolboxthat can be installed by the end-user in any of the MATLAB-compatible operating systems
For C++ we officially focus on Ubuntu long-term service(LTS) distributions in a similar way to the most recentdistributions of ROS and other large open-source libraries suchas TensorFlow12 For any Ubuntu LTS distribution that has notreached its end-of-life13 the user can install the C++ versionof the library with the following three commandssudo add-apt-repository ppadqrobotics -devreleasesudo apt-get updatesudo apt-get install libdqrobotics
made available via our release Personal Package Archive(PPA)14 in which we store the latest stable versions of thelibrary Interfaces between DQ Robotics and other librariesalso reside in the same PPA For example after addingthe PPA the user can install the V-REP interface with thefollowing commandsudo apt-get install libdqrobotics -interface -vrep
11httpsdqroboticsgithubioreadthedocsioenlatestinstallationhtml12httpswwwtensorfloworg13httpsubuntucomaboutrelease-cycle14httpslaunchpadnet~dqrobotics-dev+archiveubunturelease
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 10
The PPA guarantees that the user can reliably install thepackage in their system as the PPA only stores packages thatcompiled successfully Support for other operating systemswill be driven by user interest but should not be a bigchallenge for most systems since we are using only CMAKEand Eigen3 as project dependencies both of which are widelysupported
Lastly the Python version of DQ Robotics is made avail-able through a Pybind11-based15 Python wrapper of the C++library This has three main advantages First there is no needto redevelop and maintain versions of the libraries in differentprogramming languages which is highly demanding Secondunit-testing code written in Python which is easier to writeand maintain automatically validates the C++ library as wellThird we can have Pythonrsquos ease-of-use with C++rsquos perfor-mance for each function Although there is a computationaloverhead when using the Python bindings (in contrast withdirectly using the C++ code) it is much lower than a nativePython implementation A simple example of this can be seenin Table V in which we compare the average required timefor dual quaternion multiplications in the same machine
TABLE VCOMPUTATIONAL TIME OF DUAL-QUATERNION MULTIPLICATIONS
Mean [micros] Standard Deviation [micros]C++ (gcc 540) 022 00149
MATLAB R2019a 894 01223Python 352 Bindings 082 00242Python 352 Native 3142 01646
Mean and standard deviation of the required time for one dual-quaternion multiplicationThis was calculated from a thousand sets of a thousand executions on the same IntelCore i9 9900K Ubuntu 1604 x64 system and excludes the time required to generate therandom dual quaternions
An Ubuntu LTS user can install the Python3 version of DQRobotics from the Python Package Index (PyPI)16 with a singleline of code
python3 -m pip install --user dqrobotics
Lastly the PyPI package is continuously built from sourceusing TravisCI17 so that the code is properly compiled andtested before being distributed to our users
IX APPLICATIONS AND COMPARISON WITH OTHERPACKAGES AND LIBRARIES
DQ Robotics has been used for more than nine years tomodel and control different robots in applications such asbimanual surgical robots in constrained workspaces (Fig 10a)whole-body control of humanoid robots (Fig 10b) andthe decentralized formation control of mobile manipulators(Fig 10c)18
The main difference from other packages and librariesis that DQ Robotics implements dual quaternion algebra in
15httpsgithubcompybindpybind1116httpspypiorg17httpstravis-cicom18A more complete list of papers and applications can be found in https
dqroboticsgithubiocitationshtml
(a) The bimanual surgical robot SmartArm [12]
(b) Whole-body control of a humanoid robot [13]
(c) Cooperative mobile manipulators [14]
Fig 10 Examples of different applications of DQ Robotics
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 11
a close way to the mathematical notation Furthermore allsupported languages namely Python MATLAB and C++use the same convention and style and it is straightforwardto port code in one language to other supported languages Toenable that we avoid using features that are not available in allthree languages unless that would lead to poor-quality codeFrom the user point-of-view the advantage of such approachis that users can prototype their ideas on MATLAB usingthe convenient plot system then easily translate the code toC++ and deploy it on a real robotic platform The Pythonversion of the library is somewhere between MATLAB andC++ being suitable for prototyping and also for deploymenton real applications because C++ runs under the hood
In terms of target audience the MATLAB version of DQRobotics has some overlap with Peter Corkersquos Robotics Tool-box [15] most notably in terms of their use for educationand learning On the one hand earlier versions were greatlyinspired by Peter Corkersquos Robotics Toolbox most notablythe plot system which is particularly useful when teachingrobotics On the other hand DQ Robotics is entirely basedon general dual quaternion algebra whereas Robotics Tool-box uses classic representations for robot modeling althoughit does offer support for unit quaternions Moreover otherfunctionalities of Peter Corkersquos Robotics Toolbox such aspath planning localization and mapping are not available inDQ Robotics as of now However some functionalities ofPeter Corkersquos Robotics Toolbox are now available in toolboxesdistributed by MathWorks such as the Image ProcessingToolbox19 and the Robotics System Toolbox20 which are inprinciple compatible with DQ Robotics
Another MATLAB toolbox [7] focuses on the application ofunit dual quaternions to the neuroscience domain and presentsbasic kinematic modeling of serial mechanisms In addition tohaving many fewer functionalities than DQ Robotics it usesprocedural programming which makes the code quite differentfrom the notation used on paper
A major contrast between DQ Robotics and Peter Corkersquosand Leclercqrsquos MATLAB toolboxes is that DQ Robotics hasPython and C++ versions with identical APIs as far as theprogramming languages permit This increases the numberof possible users and considerably reduces the time requiredto go from prototyping to deployment Moreover this makesDQ Robotics compatible with powerful libraries available inPython such as SciPy21 and scikit-learn22 In addition bothPython and C++ versions of the library can be readily usedalongside ROS which is the current standard of shareablecode in the robotics community Lastly a modern developmentinfrastructure not available in other existing libraries makesthe library scalable and easier to maintain and extend
X CONCLUSIONS
This paper presented DQ Robotics a comprehensive com-putational library for robot modeling and control using dual
19httpswwwmathworkscomproductsimagehtml20httpswwwmathworkscomproductsroboticshtml21httpswwwscipyorg22httpsscikit-learnorgstable
quaternion algebra It has the unique feature of using a notationvery close to the mathematical description and supportingthree different programming languages namely MATLABPython and C++ while using very similar conventions amongthem making it very easy to switch between languages andthus shortening the development time from prototyping tothe implementation on actual robots The library currentlysupports serial manipulator robots cooperative systems suchas two-arm robots mobile manipulators and even branchedmechanisms such as humanoids Robot models are generatedautomatically from simple geometrical parameters such asthe Denavit-Hartenberg parameters In addition robots canbe easily combined to yield more complex ones while thecorresponding forward kinematics and differential kinematicsare also computed automatically and analytically at executiontime The library also offers a rich set of motion controllersand it is very simple to implement new ones Although it doesnot offer dynamics modeling yet it can be easily integrated toexisting libraries for that purpose However robot dynamicsmodeling using dual quaternion algebra is currently underdevelopment and will be integrated into the library as soonas it becomes mature Rather than being a competitor toexisting libraries DQ Robotics aims at complementing themand helping in the popularization of dual quaternion algebrain the robotics domain
ACKNOWLEDGMENT
The authors would like to thank all users of the DQ Roboticslibrary for their valuable feedback and bug reports speciallyJuan Joseacute Quiroz-Omantildea and other members of the MACROresearch group at UFMG
REFERENCES
[1] O Bottema and B Roth Theoretical kinematics North-HollandPublishing Company 1979 vol 24
[2] J McCarthy Introduction to theoretical kinematics 1st ed The MITPress 1990
[3] B V Adorno ldquoTwo-arm Manipulation From Manipulators to EnhancedHuman-Robot Collaboration [Contribution agrave la manipulation agrave deuxbras des manipulateurs agrave la collaboration homme-robot]rdquo PhDDissertation Universiteacute Montpellier 2 2011 [Online] Availablehttpstelarchives-ouvertesfrtel-00641678
[4] M W Spong and M Vidyasagar Robot dynamics and control JohnWiley amp Sons 2008
[5] B Siciliano L Sciavicco L Villani and G Oriolo Robotics modellingplanning and control Springer Science amp Business Media 2010
[6] B Siciliano and O Khatib Springer handbook of robotics Springer2016
[7] G Leclercq P Lefegravevre and G Blohm ldquo3D kinematics using dualquaternions theory and applications in neurosciencerdquo Frontiers inBehavioral Neuroscience vol 7 no February p 7 jan 2013 [Online]Available httpwwwncbinlmnihgovpubmed23443667httpjournalfrontiersinorgarticle103389fnbeh201300007abstract
[8] B V Adorno ldquoRobot Kinematic Modeling and Control Based on DualQuaternion Algebra ndash Part I Fundamentalsrdquo 2017 [Online] Availablehttpshalarchives-ouvertesfrhal-01478225v1
[9] M Quigley K Conley B Gerkey J Faust T Foote J LeibsR Wheeler and A Y Ng ldquoRos an open-source robot operating systemrdquoin ICRA workshop on open source software vol 3 no 32 Kobe Japan2009 p 5
[10] E Rohmer S P N Singh and M Freese ldquoV-REP A versatile andscalable robot simulation frameworkrdquo in 2013 IEEERSJ InternationalConference on Intelligent Robots and Systems IEEE nov 2013
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 12
[11] M M Marinho B V Adorno K Harada and M Mitsuishi ldquoDynamicActive Constraints for Surgical Robots Using Vector-Field InequalitiesrdquoIEEE Transactions on Robotics vol 35 no 5 pp 1166ndash1185 oct 2019[Online] Available httpsieeexploreieeeorgdocument8742769
[12] M M Marinho K Harada A Morita and M Mitsuishi ldquoSmartarmIntegration and validation of a versatile surgical robotic system forconstrained workspacesrdquo The International Journal of Medical Roboticsand Computer Assisted Surgery (in press) 2019
[13] J J Quiroz-Omana and B V Adorno ldquoWhole-Body Control With(Self) Collision Avoidance Using Vector Field Inequalitiesrdquo IEEERobotics and Automation Letters vol 4 no 4 pp 4048ndash4053 oct 2019
[Online] Available httpsieeexploreieeeorgdocument8763977[14] H J Savino L C Pimenta J A Shah and B V Adorno
ldquoPose consensus based on dual quaternion algebra with applicationto decentralized formation control of mobile manipulatorsrdquo Journalof the Franklin Institute pp 1ndash36 oct 2019 [Online] AvailablehttpslinkinghubelseviercomretrievepiiS0016003219307161
[15] P Corke Robotics Vision and Control 2nd ed ser SpringerTracts in Advanced Robotics Springer International Publishing2017 vol 118 [Online] Available httplinkspringercom101007978-3-319-54413-7
- I Introduction
-
- I-A How to follow the tutorial
- I-B Notation choice and common operations
-
- II Representing rigid motions
- III Robot kinematics
- IV Geometric Primitives
- V Robot motion control
- VI Interface with ROS
- VII Interface with V-REP
-
- VII-A A more complete example
-
- VIII Development infrastructure
- IX Applications and comparison with other packages and libraries
- X Conclusions
- References
-
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 9
(a) Circle partially drawn at t = 36s
(b) Circle completely drawn at t = 170s
(c) Top view showing the mobile manipulator trajectory
Fig 9 Manipulator robot interacting with a mobile manipulator in aworkspace with two cylindrical obstacles and a wall The parameters for thetrajectory generation were ωn = 01 ωd = 05 and dz = 01
End-users are agnostic to these infrastructural decisions Forthem usage performance and installation of the library matterthe most The usage of the library has been simplified byrelying on a programming syntax similar to the mathemati-
Listing 5 Main MATLAB code for the simulationsampling_time = 005total_time = 200for t=0sampling_timetotal_time
Get obstacles from V-REPplane = get_plane_from_vrep(virsquoObstaclePlanersquoDQk)cylinder1 = get_line_from_vrep(virsquoObstacleCylinder1rsquoDQk)cylinder2 = get_line_from_vrep(virsquoObstacleCylinder2rsquoDQk)
Set references for both robots[lwr4_xd lwr4_ff] = compute_lwr4_reference(lwr4
simulation_parameters lwr4_x0 t)[youbot_xd youbot_ff] = compute_youbot_reference(
youbot_control lwr4_xd lwr4_ff)
Compute the control input for the manipulatorlwr4_u = lwr4_controllercompute_tracking_control_signal(
lwr4_q vec8(lwr4_xd)vec8(lwr4_ff))
Compute constrained control input for the youbot[Jconstraint bconstraint] = compute_constraints(youbot
youbot_q planecylinder1 cylinder2)youbot_controlset_inequality_constraint(-Jconstraint
1bconstraint)youbot_u= youbot_controlcompute_tracking_control_signal(
youbot_q vec8(youbot_xd) vec8(youbot_ff))
Since we are using V-REP just for visualization integrate the control signal to update the robots configurationslwr4_q = lwr4_q + sampling_timelwr4_uyoubot_q = youbot_q + sampling_timeyoubot_u
Send desired values to V-REPlwr4_vreprobotsend_q_to_vrep(lwr4_q)youbot_vreprobotsend_q_to_vrep(youbot_q)end
cal description and being attentive to proper object-orientedprogramming practices Furthermore good performance isachieved by careful optimization of the library and by theC++ implementation
The ease of installation has been addressed in language-platform specific ways that we document in details on the DQRoboticsrsquo Read the Docs11
For MATLAB DQ Robotics is distributed as a Toolboxthat can be installed by the end-user in any of the MATLAB-compatible operating systems
For C++ we officially focus on Ubuntu long-term service(LTS) distributions in a similar way to the most recentdistributions of ROS and other large open-source libraries suchas TensorFlow12 For any Ubuntu LTS distribution that has notreached its end-of-life13 the user can install the C++ versionof the library with the following three commandssudo add-apt-repository ppadqrobotics -devreleasesudo apt-get updatesudo apt-get install libdqrobotics
made available via our release Personal Package Archive(PPA)14 in which we store the latest stable versions of thelibrary Interfaces between DQ Robotics and other librariesalso reside in the same PPA For example after addingthe PPA the user can install the V-REP interface with thefollowing commandsudo apt-get install libdqrobotics -interface -vrep
11httpsdqroboticsgithubioreadthedocsioenlatestinstallationhtml12httpswwwtensorfloworg13httpsubuntucomaboutrelease-cycle14httpslaunchpadnet~dqrobotics-dev+archiveubunturelease
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 10
The PPA guarantees that the user can reliably install thepackage in their system as the PPA only stores packages thatcompiled successfully Support for other operating systemswill be driven by user interest but should not be a bigchallenge for most systems since we are using only CMAKEand Eigen3 as project dependencies both of which are widelysupported
Lastly the Python version of DQ Robotics is made avail-able through a Pybind11-based15 Python wrapper of the C++library This has three main advantages First there is no needto redevelop and maintain versions of the libraries in differentprogramming languages which is highly demanding Secondunit-testing code written in Python which is easier to writeand maintain automatically validates the C++ library as wellThird we can have Pythonrsquos ease-of-use with C++rsquos perfor-mance for each function Although there is a computationaloverhead when using the Python bindings (in contrast withdirectly using the C++ code) it is much lower than a nativePython implementation A simple example of this can be seenin Table V in which we compare the average required timefor dual quaternion multiplications in the same machine
TABLE VCOMPUTATIONAL TIME OF DUAL-QUATERNION MULTIPLICATIONS
Mean [micros] Standard Deviation [micros]C++ (gcc 540) 022 00149
MATLAB R2019a 894 01223Python 352 Bindings 082 00242Python 352 Native 3142 01646
Mean and standard deviation of the required time for one dual-quaternion multiplicationThis was calculated from a thousand sets of a thousand executions on the same IntelCore i9 9900K Ubuntu 1604 x64 system and excludes the time required to generate therandom dual quaternions
An Ubuntu LTS user can install the Python3 version of DQRobotics from the Python Package Index (PyPI)16 with a singleline of code
python3 -m pip install --user dqrobotics
Lastly the PyPI package is continuously built from sourceusing TravisCI17 so that the code is properly compiled andtested before being distributed to our users
IX APPLICATIONS AND COMPARISON WITH OTHERPACKAGES AND LIBRARIES
DQ Robotics has been used for more than nine years tomodel and control different robots in applications such asbimanual surgical robots in constrained workspaces (Fig 10a)whole-body control of humanoid robots (Fig 10b) andthe decentralized formation control of mobile manipulators(Fig 10c)18
The main difference from other packages and librariesis that DQ Robotics implements dual quaternion algebra in
15httpsgithubcompybindpybind1116httpspypiorg17httpstravis-cicom18A more complete list of papers and applications can be found in https
dqroboticsgithubiocitationshtml
(a) The bimanual surgical robot SmartArm [12]
(b) Whole-body control of a humanoid robot [13]
(c) Cooperative mobile manipulators [14]
Fig 10 Examples of different applications of DQ Robotics
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 11
a close way to the mathematical notation Furthermore allsupported languages namely Python MATLAB and C++use the same convention and style and it is straightforwardto port code in one language to other supported languages Toenable that we avoid using features that are not available in allthree languages unless that would lead to poor-quality codeFrom the user point-of-view the advantage of such approachis that users can prototype their ideas on MATLAB usingthe convenient plot system then easily translate the code toC++ and deploy it on a real robotic platform The Pythonversion of the library is somewhere between MATLAB andC++ being suitable for prototyping and also for deploymenton real applications because C++ runs under the hood
In terms of target audience the MATLAB version of DQRobotics has some overlap with Peter Corkersquos Robotics Tool-box [15] most notably in terms of their use for educationand learning On the one hand earlier versions were greatlyinspired by Peter Corkersquos Robotics Toolbox most notablythe plot system which is particularly useful when teachingrobotics On the other hand DQ Robotics is entirely basedon general dual quaternion algebra whereas Robotics Tool-box uses classic representations for robot modeling althoughit does offer support for unit quaternions Moreover otherfunctionalities of Peter Corkersquos Robotics Toolbox such aspath planning localization and mapping are not available inDQ Robotics as of now However some functionalities ofPeter Corkersquos Robotics Toolbox are now available in toolboxesdistributed by MathWorks such as the Image ProcessingToolbox19 and the Robotics System Toolbox20 which are inprinciple compatible with DQ Robotics
Another MATLAB toolbox [7] focuses on the application ofunit dual quaternions to the neuroscience domain and presentsbasic kinematic modeling of serial mechanisms In addition tohaving many fewer functionalities than DQ Robotics it usesprocedural programming which makes the code quite differentfrom the notation used on paper
A major contrast between DQ Robotics and Peter Corkersquosand Leclercqrsquos MATLAB toolboxes is that DQ Robotics hasPython and C++ versions with identical APIs as far as theprogramming languages permit This increases the numberof possible users and considerably reduces the time requiredto go from prototyping to deployment Moreover this makesDQ Robotics compatible with powerful libraries available inPython such as SciPy21 and scikit-learn22 In addition bothPython and C++ versions of the library can be readily usedalongside ROS which is the current standard of shareablecode in the robotics community Lastly a modern developmentinfrastructure not available in other existing libraries makesthe library scalable and easier to maintain and extend
X CONCLUSIONS
This paper presented DQ Robotics a comprehensive com-putational library for robot modeling and control using dual
19httpswwwmathworkscomproductsimagehtml20httpswwwmathworkscomproductsroboticshtml21httpswwwscipyorg22httpsscikit-learnorgstable
quaternion algebra It has the unique feature of using a notationvery close to the mathematical description and supportingthree different programming languages namely MATLABPython and C++ while using very similar conventions amongthem making it very easy to switch between languages andthus shortening the development time from prototyping tothe implementation on actual robots The library currentlysupports serial manipulator robots cooperative systems suchas two-arm robots mobile manipulators and even branchedmechanisms such as humanoids Robot models are generatedautomatically from simple geometrical parameters such asthe Denavit-Hartenberg parameters In addition robots canbe easily combined to yield more complex ones while thecorresponding forward kinematics and differential kinematicsare also computed automatically and analytically at executiontime The library also offers a rich set of motion controllersand it is very simple to implement new ones Although it doesnot offer dynamics modeling yet it can be easily integrated toexisting libraries for that purpose However robot dynamicsmodeling using dual quaternion algebra is currently underdevelopment and will be integrated into the library as soonas it becomes mature Rather than being a competitor toexisting libraries DQ Robotics aims at complementing themand helping in the popularization of dual quaternion algebrain the robotics domain
ACKNOWLEDGMENT
The authors would like to thank all users of the DQ Roboticslibrary for their valuable feedback and bug reports speciallyJuan Joseacute Quiroz-Omantildea and other members of the MACROresearch group at UFMG
REFERENCES
[1] O Bottema and B Roth Theoretical kinematics North-HollandPublishing Company 1979 vol 24
[2] J McCarthy Introduction to theoretical kinematics 1st ed The MITPress 1990
[3] B V Adorno ldquoTwo-arm Manipulation From Manipulators to EnhancedHuman-Robot Collaboration [Contribution agrave la manipulation agrave deuxbras des manipulateurs agrave la collaboration homme-robot]rdquo PhDDissertation Universiteacute Montpellier 2 2011 [Online] Availablehttpstelarchives-ouvertesfrtel-00641678
[4] M W Spong and M Vidyasagar Robot dynamics and control JohnWiley amp Sons 2008
[5] B Siciliano L Sciavicco L Villani and G Oriolo Robotics modellingplanning and control Springer Science amp Business Media 2010
[6] B Siciliano and O Khatib Springer handbook of robotics Springer2016
[7] G Leclercq P Lefegravevre and G Blohm ldquo3D kinematics using dualquaternions theory and applications in neurosciencerdquo Frontiers inBehavioral Neuroscience vol 7 no February p 7 jan 2013 [Online]Available httpwwwncbinlmnihgovpubmed23443667httpjournalfrontiersinorgarticle103389fnbeh201300007abstract
[8] B V Adorno ldquoRobot Kinematic Modeling and Control Based on DualQuaternion Algebra ndash Part I Fundamentalsrdquo 2017 [Online] Availablehttpshalarchives-ouvertesfrhal-01478225v1
[9] M Quigley K Conley B Gerkey J Faust T Foote J LeibsR Wheeler and A Y Ng ldquoRos an open-source robot operating systemrdquoin ICRA workshop on open source software vol 3 no 32 Kobe Japan2009 p 5
[10] E Rohmer S P N Singh and M Freese ldquoV-REP A versatile andscalable robot simulation frameworkrdquo in 2013 IEEERSJ InternationalConference on Intelligent Robots and Systems IEEE nov 2013
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 12
[11] M M Marinho B V Adorno K Harada and M Mitsuishi ldquoDynamicActive Constraints for Surgical Robots Using Vector-Field InequalitiesrdquoIEEE Transactions on Robotics vol 35 no 5 pp 1166ndash1185 oct 2019[Online] Available httpsieeexploreieeeorgdocument8742769
[12] M M Marinho K Harada A Morita and M Mitsuishi ldquoSmartarmIntegration and validation of a versatile surgical robotic system forconstrained workspacesrdquo The International Journal of Medical Roboticsand Computer Assisted Surgery (in press) 2019
[13] J J Quiroz-Omana and B V Adorno ldquoWhole-Body Control With(Self) Collision Avoidance Using Vector Field Inequalitiesrdquo IEEERobotics and Automation Letters vol 4 no 4 pp 4048ndash4053 oct 2019
[Online] Available httpsieeexploreieeeorgdocument8763977[14] H J Savino L C Pimenta J A Shah and B V Adorno
ldquoPose consensus based on dual quaternion algebra with applicationto decentralized formation control of mobile manipulatorsrdquo Journalof the Franklin Institute pp 1ndash36 oct 2019 [Online] AvailablehttpslinkinghubelseviercomretrievepiiS0016003219307161
[15] P Corke Robotics Vision and Control 2nd ed ser SpringerTracts in Advanced Robotics Springer International Publishing2017 vol 118 [Online] Available httplinkspringercom101007978-3-319-54413-7
- I Introduction
-
- I-A How to follow the tutorial
- I-B Notation choice and common operations
-
- II Representing rigid motions
- III Robot kinematics
- IV Geometric Primitives
- V Robot motion control
- VI Interface with ROS
- VII Interface with V-REP
-
- VII-A A more complete example
-
- VIII Development infrastructure
- IX Applications and comparison with other packages and libraries
- X Conclusions
- References
-
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 10
The PPA guarantees that the user can reliably install thepackage in their system as the PPA only stores packages thatcompiled successfully Support for other operating systemswill be driven by user interest but should not be a bigchallenge for most systems since we are using only CMAKEand Eigen3 as project dependencies both of which are widelysupported
Lastly the Python version of DQ Robotics is made avail-able through a Pybind11-based15 Python wrapper of the C++library This has three main advantages First there is no needto redevelop and maintain versions of the libraries in differentprogramming languages which is highly demanding Secondunit-testing code written in Python which is easier to writeand maintain automatically validates the C++ library as wellThird we can have Pythonrsquos ease-of-use with C++rsquos perfor-mance for each function Although there is a computationaloverhead when using the Python bindings (in contrast withdirectly using the C++ code) it is much lower than a nativePython implementation A simple example of this can be seenin Table V in which we compare the average required timefor dual quaternion multiplications in the same machine
TABLE VCOMPUTATIONAL TIME OF DUAL-QUATERNION MULTIPLICATIONS
Mean [micros] Standard Deviation [micros]C++ (gcc 540) 022 00149
MATLAB R2019a 894 01223Python 352 Bindings 082 00242Python 352 Native 3142 01646
Mean and standard deviation of the required time for one dual-quaternion multiplicationThis was calculated from a thousand sets of a thousand executions on the same IntelCore i9 9900K Ubuntu 1604 x64 system and excludes the time required to generate therandom dual quaternions
An Ubuntu LTS user can install the Python3 version of DQRobotics from the Python Package Index (PyPI)16 with a singleline of code
python3 -m pip install --user dqrobotics
Lastly the PyPI package is continuously built from sourceusing TravisCI17 so that the code is properly compiled andtested before being distributed to our users
IX APPLICATIONS AND COMPARISON WITH OTHERPACKAGES AND LIBRARIES
DQ Robotics has been used for more than nine years tomodel and control different robots in applications such asbimanual surgical robots in constrained workspaces (Fig 10a)whole-body control of humanoid robots (Fig 10b) andthe decentralized formation control of mobile manipulators(Fig 10c)18
The main difference from other packages and librariesis that DQ Robotics implements dual quaternion algebra in
15httpsgithubcompybindpybind1116httpspypiorg17httpstravis-cicom18A more complete list of papers and applications can be found in https
dqroboticsgithubiocitationshtml
(a) The bimanual surgical robot SmartArm [12]
(b) Whole-body control of a humanoid robot [13]
(c) Cooperative mobile manipulators [14]
Fig 10 Examples of different applications of DQ Robotics
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 11
a close way to the mathematical notation Furthermore allsupported languages namely Python MATLAB and C++use the same convention and style and it is straightforwardto port code in one language to other supported languages Toenable that we avoid using features that are not available in allthree languages unless that would lead to poor-quality codeFrom the user point-of-view the advantage of such approachis that users can prototype their ideas on MATLAB usingthe convenient plot system then easily translate the code toC++ and deploy it on a real robotic platform The Pythonversion of the library is somewhere between MATLAB andC++ being suitable for prototyping and also for deploymenton real applications because C++ runs under the hood
In terms of target audience the MATLAB version of DQRobotics has some overlap with Peter Corkersquos Robotics Tool-box [15] most notably in terms of their use for educationand learning On the one hand earlier versions were greatlyinspired by Peter Corkersquos Robotics Toolbox most notablythe plot system which is particularly useful when teachingrobotics On the other hand DQ Robotics is entirely basedon general dual quaternion algebra whereas Robotics Tool-box uses classic representations for robot modeling althoughit does offer support for unit quaternions Moreover otherfunctionalities of Peter Corkersquos Robotics Toolbox such aspath planning localization and mapping are not available inDQ Robotics as of now However some functionalities ofPeter Corkersquos Robotics Toolbox are now available in toolboxesdistributed by MathWorks such as the Image ProcessingToolbox19 and the Robotics System Toolbox20 which are inprinciple compatible with DQ Robotics
Another MATLAB toolbox [7] focuses on the application ofunit dual quaternions to the neuroscience domain and presentsbasic kinematic modeling of serial mechanisms In addition tohaving many fewer functionalities than DQ Robotics it usesprocedural programming which makes the code quite differentfrom the notation used on paper
A major contrast between DQ Robotics and Peter Corkersquosand Leclercqrsquos MATLAB toolboxes is that DQ Robotics hasPython and C++ versions with identical APIs as far as theprogramming languages permit This increases the numberof possible users and considerably reduces the time requiredto go from prototyping to deployment Moreover this makesDQ Robotics compatible with powerful libraries available inPython such as SciPy21 and scikit-learn22 In addition bothPython and C++ versions of the library can be readily usedalongside ROS which is the current standard of shareablecode in the robotics community Lastly a modern developmentinfrastructure not available in other existing libraries makesthe library scalable and easier to maintain and extend
X CONCLUSIONS
This paper presented DQ Robotics a comprehensive com-putational library for robot modeling and control using dual
19httpswwwmathworkscomproductsimagehtml20httpswwwmathworkscomproductsroboticshtml21httpswwwscipyorg22httpsscikit-learnorgstable
quaternion algebra It has the unique feature of using a notationvery close to the mathematical description and supportingthree different programming languages namely MATLABPython and C++ while using very similar conventions amongthem making it very easy to switch between languages andthus shortening the development time from prototyping tothe implementation on actual robots The library currentlysupports serial manipulator robots cooperative systems suchas two-arm robots mobile manipulators and even branchedmechanisms such as humanoids Robot models are generatedautomatically from simple geometrical parameters such asthe Denavit-Hartenberg parameters In addition robots canbe easily combined to yield more complex ones while thecorresponding forward kinematics and differential kinematicsare also computed automatically and analytically at executiontime The library also offers a rich set of motion controllersand it is very simple to implement new ones Although it doesnot offer dynamics modeling yet it can be easily integrated toexisting libraries for that purpose However robot dynamicsmodeling using dual quaternion algebra is currently underdevelopment and will be integrated into the library as soonas it becomes mature Rather than being a competitor toexisting libraries DQ Robotics aims at complementing themand helping in the popularization of dual quaternion algebrain the robotics domain
ACKNOWLEDGMENT
The authors would like to thank all users of the DQ Roboticslibrary for their valuable feedback and bug reports speciallyJuan Joseacute Quiroz-Omantildea and other members of the MACROresearch group at UFMG
REFERENCES
[1] O Bottema and B Roth Theoretical kinematics North-HollandPublishing Company 1979 vol 24
[2] J McCarthy Introduction to theoretical kinematics 1st ed The MITPress 1990
[3] B V Adorno ldquoTwo-arm Manipulation From Manipulators to EnhancedHuman-Robot Collaboration [Contribution agrave la manipulation agrave deuxbras des manipulateurs agrave la collaboration homme-robot]rdquo PhDDissertation Universiteacute Montpellier 2 2011 [Online] Availablehttpstelarchives-ouvertesfrtel-00641678
[4] M W Spong and M Vidyasagar Robot dynamics and control JohnWiley amp Sons 2008
[5] B Siciliano L Sciavicco L Villani and G Oriolo Robotics modellingplanning and control Springer Science amp Business Media 2010
[6] B Siciliano and O Khatib Springer handbook of robotics Springer2016
[7] G Leclercq P Lefegravevre and G Blohm ldquo3D kinematics using dualquaternions theory and applications in neurosciencerdquo Frontiers inBehavioral Neuroscience vol 7 no February p 7 jan 2013 [Online]Available httpwwwncbinlmnihgovpubmed23443667httpjournalfrontiersinorgarticle103389fnbeh201300007abstract
[8] B V Adorno ldquoRobot Kinematic Modeling and Control Based on DualQuaternion Algebra ndash Part I Fundamentalsrdquo 2017 [Online] Availablehttpshalarchives-ouvertesfrhal-01478225v1
[9] M Quigley K Conley B Gerkey J Faust T Foote J LeibsR Wheeler and A Y Ng ldquoRos an open-source robot operating systemrdquoin ICRA workshop on open source software vol 3 no 32 Kobe Japan2009 p 5
[10] E Rohmer S P N Singh and M Freese ldquoV-REP A versatile andscalable robot simulation frameworkrdquo in 2013 IEEERSJ InternationalConference on Intelligent Robots and Systems IEEE nov 2013
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 12
[11] M M Marinho B V Adorno K Harada and M Mitsuishi ldquoDynamicActive Constraints for Surgical Robots Using Vector-Field InequalitiesrdquoIEEE Transactions on Robotics vol 35 no 5 pp 1166ndash1185 oct 2019[Online] Available httpsieeexploreieeeorgdocument8742769
[12] M M Marinho K Harada A Morita and M Mitsuishi ldquoSmartarmIntegration and validation of a versatile surgical robotic system forconstrained workspacesrdquo The International Journal of Medical Roboticsand Computer Assisted Surgery (in press) 2019
[13] J J Quiroz-Omana and B V Adorno ldquoWhole-Body Control With(Self) Collision Avoidance Using Vector Field Inequalitiesrdquo IEEERobotics and Automation Letters vol 4 no 4 pp 4048ndash4053 oct 2019
[Online] Available httpsieeexploreieeeorgdocument8763977[14] H J Savino L C Pimenta J A Shah and B V Adorno
ldquoPose consensus based on dual quaternion algebra with applicationto decentralized formation control of mobile manipulatorsrdquo Journalof the Franklin Institute pp 1ndash36 oct 2019 [Online] AvailablehttpslinkinghubelseviercomretrievepiiS0016003219307161
[15] P Corke Robotics Vision and Control 2nd ed ser SpringerTracts in Advanced Robotics Springer International Publishing2017 vol 118 [Online] Available httplinkspringercom101007978-3-319-54413-7
- I Introduction
-
- I-A How to follow the tutorial
- I-B Notation choice and common operations
-
- II Representing rigid motions
- III Robot kinematics
- IV Geometric Primitives
- V Robot motion control
- VI Interface with ROS
- VII Interface with V-REP
-
- VII-A A more complete example
-
- VIII Development infrastructure
- IX Applications and comparison with other packages and libraries
- X Conclusions
- References
-
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 11
a close way to the mathematical notation Furthermore allsupported languages namely Python MATLAB and C++use the same convention and style and it is straightforwardto port code in one language to other supported languages Toenable that we avoid using features that are not available in allthree languages unless that would lead to poor-quality codeFrom the user point-of-view the advantage of such approachis that users can prototype their ideas on MATLAB usingthe convenient plot system then easily translate the code toC++ and deploy it on a real robotic platform The Pythonversion of the library is somewhere between MATLAB andC++ being suitable for prototyping and also for deploymenton real applications because C++ runs under the hood
In terms of target audience the MATLAB version of DQRobotics has some overlap with Peter Corkersquos Robotics Tool-box [15] most notably in terms of their use for educationand learning On the one hand earlier versions were greatlyinspired by Peter Corkersquos Robotics Toolbox most notablythe plot system which is particularly useful when teachingrobotics On the other hand DQ Robotics is entirely basedon general dual quaternion algebra whereas Robotics Tool-box uses classic representations for robot modeling althoughit does offer support for unit quaternions Moreover otherfunctionalities of Peter Corkersquos Robotics Toolbox such aspath planning localization and mapping are not available inDQ Robotics as of now However some functionalities ofPeter Corkersquos Robotics Toolbox are now available in toolboxesdistributed by MathWorks such as the Image ProcessingToolbox19 and the Robotics System Toolbox20 which are inprinciple compatible with DQ Robotics
Another MATLAB toolbox [7] focuses on the application ofunit dual quaternions to the neuroscience domain and presentsbasic kinematic modeling of serial mechanisms In addition tohaving many fewer functionalities than DQ Robotics it usesprocedural programming which makes the code quite differentfrom the notation used on paper
A major contrast between DQ Robotics and Peter Corkersquosand Leclercqrsquos MATLAB toolboxes is that DQ Robotics hasPython and C++ versions with identical APIs as far as theprogramming languages permit This increases the numberof possible users and considerably reduces the time requiredto go from prototyping to deployment Moreover this makesDQ Robotics compatible with powerful libraries available inPython such as SciPy21 and scikit-learn22 In addition bothPython and C++ versions of the library can be readily usedalongside ROS which is the current standard of shareablecode in the robotics community Lastly a modern developmentinfrastructure not available in other existing libraries makesthe library scalable and easier to maintain and extend
X CONCLUSIONS
This paper presented DQ Robotics a comprehensive com-putational library for robot modeling and control using dual
19httpswwwmathworkscomproductsimagehtml20httpswwwmathworkscomproductsroboticshtml21httpswwwscipyorg22httpsscikit-learnorgstable
quaternion algebra It has the unique feature of using a notationvery close to the mathematical description and supportingthree different programming languages namely MATLABPython and C++ while using very similar conventions amongthem making it very easy to switch between languages andthus shortening the development time from prototyping tothe implementation on actual robots The library currentlysupports serial manipulator robots cooperative systems suchas two-arm robots mobile manipulators and even branchedmechanisms such as humanoids Robot models are generatedautomatically from simple geometrical parameters such asthe Denavit-Hartenberg parameters In addition robots canbe easily combined to yield more complex ones while thecorresponding forward kinematics and differential kinematicsare also computed automatically and analytically at executiontime The library also offers a rich set of motion controllersand it is very simple to implement new ones Although it doesnot offer dynamics modeling yet it can be easily integrated toexisting libraries for that purpose However robot dynamicsmodeling using dual quaternion algebra is currently underdevelopment and will be integrated into the library as soonas it becomes mature Rather than being a competitor toexisting libraries DQ Robotics aims at complementing themand helping in the popularization of dual quaternion algebrain the robotics domain
ACKNOWLEDGMENT
The authors would like to thank all users of the DQ Roboticslibrary for their valuable feedback and bug reports speciallyJuan Joseacute Quiroz-Omantildea and other members of the MACROresearch group at UFMG
REFERENCES
[1] O Bottema and B Roth Theoretical kinematics North-HollandPublishing Company 1979 vol 24
[2] J McCarthy Introduction to theoretical kinematics 1st ed The MITPress 1990
[3] B V Adorno ldquoTwo-arm Manipulation From Manipulators to EnhancedHuman-Robot Collaboration [Contribution agrave la manipulation agrave deuxbras des manipulateurs agrave la collaboration homme-robot]rdquo PhDDissertation Universiteacute Montpellier 2 2011 [Online] Availablehttpstelarchives-ouvertesfrtel-00641678
[4] M W Spong and M Vidyasagar Robot dynamics and control JohnWiley amp Sons 2008
[5] B Siciliano L Sciavicco L Villani and G Oriolo Robotics modellingplanning and control Springer Science amp Business Media 2010
[6] B Siciliano and O Khatib Springer handbook of robotics Springer2016
[7] G Leclercq P Lefegravevre and G Blohm ldquo3D kinematics using dualquaternions theory and applications in neurosciencerdquo Frontiers inBehavioral Neuroscience vol 7 no February p 7 jan 2013 [Online]Available httpwwwncbinlmnihgovpubmed23443667httpjournalfrontiersinorgarticle103389fnbeh201300007abstract
[8] B V Adorno ldquoRobot Kinematic Modeling and Control Based on DualQuaternion Algebra ndash Part I Fundamentalsrdquo 2017 [Online] Availablehttpshalarchives-ouvertesfrhal-01478225v1
[9] M Quigley K Conley B Gerkey J Faust T Foote J LeibsR Wheeler and A Y Ng ldquoRos an open-source robot operating systemrdquoin ICRA workshop on open source software vol 3 no 32 Kobe Japan2009 p 5
[10] E Rohmer S P N Singh and M Freese ldquoV-REP A versatile andscalable robot simulation frameworkrdquo in 2013 IEEERSJ InternationalConference on Intelligent Robots and Systems IEEE nov 2013
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 12
[11] M M Marinho B V Adorno K Harada and M Mitsuishi ldquoDynamicActive Constraints for Surgical Robots Using Vector-Field InequalitiesrdquoIEEE Transactions on Robotics vol 35 no 5 pp 1166ndash1185 oct 2019[Online] Available httpsieeexploreieeeorgdocument8742769
[12] M M Marinho K Harada A Morita and M Mitsuishi ldquoSmartarmIntegration and validation of a versatile surgical robotic system forconstrained workspacesrdquo The International Journal of Medical Roboticsand Computer Assisted Surgery (in press) 2019
[13] J J Quiroz-Omana and B V Adorno ldquoWhole-Body Control With(Self) Collision Avoidance Using Vector Field Inequalitiesrdquo IEEERobotics and Automation Letters vol 4 no 4 pp 4048ndash4053 oct 2019
[Online] Available httpsieeexploreieeeorgdocument8763977[14] H J Savino L C Pimenta J A Shah and B V Adorno
ldquoPose consensus based on dual quaternion algebra with applicationto decentralized formation control of mobile manipulatorsrdquo Journalof the Franklin Institute pp 1ndash36 oct 2019 [Online] AvailablehttpslinkinghubelseviercomretrievepiiS0016003219307161
[15] P Corke Robotics Vision and Control 2nd ed ser SpringerTracts in Advanced Robotics Springer International Publishing2017 vol 118 [Online] Available httplinkspringercom101007978-3-319-54413-7
- I Introduction
-
- I-A How to follow the tutorial
- I-B Notation choice and common operations
-
- II Representing rigid motions
- III Robot kinematics
- IV Geometric Primitives
- V Robot motion control
- VI Interface with ROS
- VII Interface with V-REP
-
- VII-A A more complete example
-
- VIII Development infrastructure
- IX Applications and comparison with other packages and libraries
- X Conclusions
- References
-
ACCEPTED FOR PUBLICATION IN IEEE ROBOTICS amp AUTOMATION MAGAZINE 12
[11] M M Marinho B V Adorno K Harada and M Mitsuishi ldquoDynamicActive Constraints for Surgical Robots Using Vector-Field InequalitiesrdquoIEEE Transactions on Robotics vol 35 no 5 pp 1166ndash1185 oct 2019[Online] Available httpsieeexploreieeeorgdocument8742769
[12] M M Marinho K Harada A Morita and M Mitsuishi ldquoSmartarmIntegration and validation of a versatile surgical robotic system forconstrained workspacesrdquo The International Journal of Medical Roboticsand Computer Assisted Surgery (in press) 2019
[13] J J Quiroz-Omana and B V Adorno ldquoWhole-Body Control With(Self) Collision Avoidance Using Vector Field Inequalitiesrdquo IEEERobotics and Automation Letters vol 4 no 4 pp 4048ndash4053 oct 2019
[Online] Available httpsieeexploreieeeorgdocument8763977[14] H J Savino L C Pimenta J A Shah and B V Adorno
ldquoPose consensus based on dual quaternion algebra with applicationto decentralized formation control of mobile manipulatorsrdquo Journalof the Franklin Institute pp 1ndash36 oct 2019 [Online] AvailablehttpslinkinghubelseviercomretrievepiiS0016003219307161
[15] P Corke Robotics Vision and Control 2nd ed ser SpringerTracts in Advanced Robotics Springer International Publishing2017 vol 118 [Online] Available httplinkspringercom101007978-3-319-54413-7
- I Introduction
-
- I-A How to follow the tutorial
- I-B Notation choice and common operations
-
- II Representing rigid motions
- III Robot kinematics
- IV Geometric Primitives
- V Robot motion control
- VI Interface with ROS
- VII Interface with V-REP
-
- VII-A A more complete example
-
- VIII Development infrastructure
- IX Applications and comparison with other packages and libraries
- X Conclusions
- References
-