diff --git a/AsibotConfiguration_8hpp_source.html b/AsibotConfiguration_8hpp_source.html
new file mode 100644
index 000000000..ef5f8c36a
--- /dev/null
+++ b/AsibotConfiguration_8hpp_source.html
@@ -0,0 +1,224 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/AsibotSolver/AsibotConfiguration.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __ASIBOT_CONFIGURATION_HPP__
+
4 #define __ASIBOT_CONFIGURATION_HPP__
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
34 : _qMin(qMin), _qMax(qMax)
+
+
+
+
+
57 virtual bool configure (
double q1,
double q2u,
double q2d,
double q3,
double q4u,
double q4d,
double q5);
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
89 : _q1(0.0), _q2(0.0), _q3(0.0), _q4(0.0), _q5(0.0), valid(true), _orient(FORWARD), _elb(UP)
+
+
+
+
+
+
+
+
+
+
+
104 double _q1, _q2, _q3, _q4, _q5;
+
+
+
+
+
+
+
+
+
+
+
+
116 Pose forwardElbowDown;
+
117 Pose reversedElbowUp;
+
118 Pose reversedElbowDown;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
174 : _qMin(qMin), _qMax(qMax)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Base factory class for AsibotConfiguration.
Definition: AsibotConfiguration.hpp:158
+
AsibotConfigurationFactory(AsibotConfiguration::JointsIn qMin, AsibotConfiguration::JointsIn qMax)
Constructor.
Definition: AsibotConfiguration.hpp:173
+
virtual AsibotConfiguration * create() const =0
Creates an instance of the concrete class.
+
Implementation factory class for AsibotConfigurationLeastOverallAngularDisplacement.
Definition: AsibotConfiguration.hpp:187
+
AsibotConfiguration * create() const override
Creates an instance of the concrete class.
Definition: AsibotConfiguration.hpp:198
+
AsibotConfigurationLeastOverallAngularDisplacementFactory(AsibotConfiguration::JointsIn qMin, AsibotConfiguration::JointsIn qMax)
Constructor.
Definition: AsibotConfiguration.hpp:194
+
IK solver configuration strategy selector based on the overall angle displacement of all joints.
Definition: AsibotConfiguration.hpp:132
+
std::vector< double > getDiffs(JointsIn qGuess, const Pose &pose)
Obtains vector of differences between current and desired joint angles [deg].
Definition: AsibotConfigurationLeastOverallAngularDisplacement.cpp:85
+
AsibotConfigurationLeastOverallAngularDisplacement(JointsIn qMin, JointsIn qMax)
Constructor.
Definition: AsibotConfiguration.hpp:139
+
bool findOptimalConfiguration(JointsIn qGuess) override
Analyzes available configurations and selects the optimal one.
Definition: AsibotConfigurationLeastOverallAngularDisplacement.cpp:16
+
Abstract base class for a robot configuration strategy selector.
Definition: AsibotConfiguration.hpp:20
+
virtual ~AsibotConfiguration()=default
Destructor.
+
virtual bool findOptimalConfiguration(JointsIn qGuess)=0
Analyzes available configurations and selects the optimal one.
+
AsibotConfiguration(JointsIn qMin, JointsIn qMax)
Constructor.
Definition: AsibotConfiguration.hpp:33
+
virtual bool configure(double q1, double q2u, double q2d, double q3, double q4u, double q4d, double q5)
Stores initial values for a specific pose.
Definition: AsibotConfiguration.cpp:47
+
virtual void retrieveAngles(JointsOut q) const
Queries computed angles for the optimal configuration.
Definition: AsibotConfiguration.hpp:70
+
const std::vector< double > & JointsIn
Const vector of joint angles (input parameter).
Definition: AsibotConfiguration.hpp:23
+
std::vector< double > & JointsOut
Vector of joint angles (output parameter).
Definition: AsibotConfiguration.hpp:26
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
Helper class to store a specific robot configuration.
Definition: AsibotConfiguration.hpp:80
+
void retrieveAngles(JointsOut q) const
Returns stored angles.
Definition: AsibotConfiguration.cpp:126
+
bool checkJointsInLimits(JointsIn qMin, JointsIn qMax) const
Checks whether current configuration is reachable.
Definition: AsibotConfiguration.cpp:107
+
void storeAngles(double q1, double q2, double q3, double q4, double q5, orientation orient, elbow elb)
Initializes angle values (in degrees).
Definition: AsibotConfiguration.cpp:95
+
orientation
Orientation of axis 1 ('forward' or 180º offset).
Definition: AsibotConfiguration.hpp:82
+
Pose()
Constructor.
Definition: AsibotConfiguration.hpp:88
+
elbow
Orientation of axes 2-3 (elbow up/down).
Definition: AsibotConfiguration.hpp:85
+
std::string toString() const
Serializes stored data.
Definition: AsibotConfiguration.cpp:131
+
+
+
+
+
diff --git a/AsibotSolver_8hpp_source.html b/AsibotSolver_8hpp_source.html
new file mode 100644
index 000000000..6cc5c72dc
--- /dev/null
+++ b/AsibotSolver_8hpp_source.html
@@ -0,0 +1,195 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/AsibotSolver/AsibotSolver.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __ASIBOT_SOLVER_HPP__
+
4 #define __ASIBOT_SOLVER_HPP__
+
+
+
+
+
9 #include <yarp/os/Searchable.h>
+
10 #include <yarp/dev/DeviceDriver.h>
+
11 #include <yarp/sig/Matrix.h>
+
+
13 #include "AsibotConfiguration.hpp"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
43 bool appendLink (
const std::vector<double> &x)
override ;
+
+
+
+
+
+
+
50 const std::vector<double> &x_new_old,
+
51 std::vector<double> &x_new_obj)
override ;
+
+
+
54 bool fwdKin (
const std::vector<double> &q, std::vector<double> &x)
override ;
+
+
+
57 bool poseDiff (
const std::vector<double> &xLhs,
const std::vector<double> &xRhs, std::vector<double> &xOut)
override ;
+
+
+
60 bool invKin (
const std::vector<double> &xd,
const std::vector<double> &qGuess, std::vector<double> &q,
+
+
+
+
64 bool diffInvKin (
const std::vector<double> &q,
const std::vector<double> &xdot, std::vector<double> &qdot,
+
+
+
+
68 bool invDyn (
const std::vector<double> &q, std::vector<double> &t)
override ;
+
+
+
71 bool invDyn (
const std::vector<double> &q,
const std::vector<double> &qdot,
const std::vector<double> &qdotdot,
+
72 const std::vector<double> &ftip, std::vector<double> &t,
reference_frame frame)
override ;
+
+
+
75 bool open(yarp::os::Searchable& config)
override ;
+
76 bool close()
override ;
+
+
+
+
+
+
82 yarp::sig::Matrix frameTcp;
+
+
+
+
+
+
+
+
90 double A0, A1, A2, A3;
+
+
92 std::vector<double> qMin, qMax;
+
+
+
+
96 AsibotTcpFrame tcpFrameStruct;
+
+
98 mutable std::mutex mtx;
+
+
+
+
+
+
Contains roboticslab::ICartesianSolver.
+
Base factory class for AsibotConfiguration.
Definition: AsibotConfiguration.hpp:158
+
Abstract base class for a robot configuration strategy selector.
Definition: AsibotConfiguration.hpp:20
+
The AsibotSolver implements ICartesianSolver.
Definition: AsibotSolver.hpp:32
+
bool diffInvKin(const std::vector< double > &q, const std::vector< double > &xdot, std::vector< double > &qdot, reference_frame frame) override
Perform differential inverse kinematics.
Definition: ICartesianSolverImpl.cpp:421
+
bool fwdKin(const std::vector< double > &q, std::vector< double > &x) override
Perform forward kinematics.
Definition: ICartesianSolverImpl.cpp:240
+
bool restoreOriginalChain() override
Restore original kinematic chain.
Definition: ICartesianSolverImpl.cpp:216
+
int getNumTcps() override
Get number of TCPs for which the solver has been configured.
Definition: ICartesianSolverImpl.cpp:195
+
bool poseDiff(const std::vector< double > &xLhs, const std::vector< double > &xRhs, std::vector< double > &xOut) override
Obtain difference between supplied pose inputs.
Definition: ICartesianSolverImpl.cpp:288
+
bool invDyn(const std::vector< double > &q, std::vector< double > &t) override
Perform inverse dynamics.
Definition: ICartesianSolverImpl.cpp:487
+
bool invKin(const std::vector< double > &xd, const std::vector< double > &qGuess, std::vector< double > &q, reference_frame frame) override
Perform inverse kinematics.
Definition: ICartesianSolverImpl.cpp:314
+
bool changeOrigin(const std::vector< double > &x_old_obj, const std::vector< double > &x_new_old, std::vector< double > &x_new_obj) override
Change origin in which a pose is expressed.
Definition: ICartesianSolverImpl.cpp:227
+
int getNumJoints() override
Get number of joints for which the solver has been configured.
Definition: ICartesianSolverImpl.cpp:188
+
bool appendLink(const std::vector< double > &x) override
Append an additional link.
Definition: ICartesianSolverImpl.cpp:202
+
Abstract base class for a cartesian solver.
Definition: ICartesianSolver.h:23
+
reference_frame
Lists supported reference frames.
Definition: ICartesianSolver.h:27
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
Definition: AsibotSolver.hpp:80
+
+
+
+
+
diff --git a/BasicCartesianControl_8hpp_source.html b/BasicCartesianControl_8hpp_source.html
new file mode 100644
index 000000000..fc0d8915c
--- /dev/null
+++ b/BasicCartesianControl_8hpp_source.html
@@ -0,0 +1,263 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __BASIC_CARTESIAN_CONTROL_HPP__
+
4 #define __BASIC_CARTESIAN_CONTROL_HPP__
+
+
+
+
+
+
+
+
+
13 #include <yarp/os/PeriodicThread.h>
+
+
15 #include <yarp/dev/DeviceDriver.h>
+
16 #include <yarp/dev/PolyDriver.h>
+
17 #include <yarp/dev/ControlBoardInterfaces.h>
+
18 #include <yarp/dev/IPreciselyTimed.h>
+
+
20 #include <kdl/trajectory.hpp>
+
+
+
+
+
+
+
+
+
108 public yarp::os::PeriodicThread,
+
+
+
+
+
+
+
+
116 bool stat (std::vector<double> & x,
int * state =
nullptr ,
double * timestamp =
nullptr )
override ;
+
117 bool inv (
const std::vector<double> & xd, std::vector<double> & q)
override ;
+
118 bool movj (
const std::vector<double> & xd)
override ;
+
119 bool relj (
const std::vector<double> & xd)
override ;
+
120 bool movl (
const std::vector<double> & xd)
override ;
+
121 bool movv (
const std::vector<double> & xdotd)
override ;
+
122 bool gcmp ()
override ;
+
123 bool forc (
const std::vector<double> &
fd )
override ;
+
+
125 bool wait (
double timeout)
override ;
+
126 bool tool (
const std::vector<double> & x)
override ;
+
127 bool act (
int command)
override ;
+
128 void pose (
const std::vector<double> & x)
override ;
+
129 void twist (
const std::vector<double> & xdot)
override ;
+
130 void wrench (
const std::vector<double> & w)
override ;
+
+
+
133 bool setParameters (
const std::map<int, double> & params)
override ;
+
+
+
+
+
+
+
140 bool open(yarp::os::Searchable & config)
override ;
+
141 bool close()
override ;
+
+
+
+
+
+
147 template <
typename Fn>
+
+
+
+
+
152 {
if (handler) handler(); }
+
+
154 void suppress()
const
+
155 { handler =
nullptr ; }
+
+
+
158 mutable std::function<void()> handler;
+
+
+
161 int getCurrentState()
const ;
+
162 void setCurrentState(
int value);
+
+
164 bool checkJointLimits(
const std::vector<double> & q);
+
165 bool checkJointLimits(
const std::vector<double> & q,
const std::vector<double> & qdot);
+
166 bool checkJointVelocities(
const std::vector<double> & qdot);
+
167 bool doFailFastChecks(
const std::vector<double> & initialQ);
+
168 bool checkControlModes(
int mode);
+
169 bool setControlModes(
int mode);
+
170 bool presetStreamingCommand(
int command);
+
171 bool computeIsocronousSpeeds(
const std::vector<double> & q,
const std::vector<double> & qd, std::vector<double> & qdot);
+
+
173 void handleMovj(
const std::vector<double> & q,
const StateWatcher & watcher);
+
174 void handleMovlVel(
const std::vector<double> & q,
const StateWatcher & watcher);
+
175 void handleMovlPosd(
const std::vector<double> & q,
const StateWatcher & watcher);
+
176 void handleMovv(
const std::vector<double> & q,
const StateWatcher & watcher);
+
177 void handleGcmp(
const std::vector<double> & q,
const StateWatcher & watcher);
+
178 void handleForc(
const std::vector<double> & q,
const std::vector<double> & qdot,
const std::vector<double> & qdotdot,
const StateWatcher & watcher);
+
+
180 yarp::dev::PolyDriver solverDevice;
+
+
+
183 yarp::dev::PolyDriver robotDevice;
+
184 yarp::dev::IControlMode * iControlMode {
nullptr };
+
185 yarp::dev::IEncoders * iEncoders {
nullptr };
+
186 yarp::dev::IPositionControl * iPositionControl {
nullptr };
+
187 yarp::dev::IPositionDirect * iPositionDirect {
nullptr };
+
188 yarp::dev::IPreciselyTimed * iPreciselyTimed {
nullptr };
+
189 yarp::dev::ITorqueControl * iTorqueControl {
nullptr };
+
190 yarp::dev::IVelocityControl * iVelocityControl {
nullptr };
+
+
+
+
+
+
+
+
+
+
+
201 int streamingCommand;
+
+
+
+
+
206 mutable std::mutex stateMutex;
+
+
+
+
+
+
+
+
218 std::vector<double>
fd ;
+
+
220 int encoderErrors {0};
+
221 std::atomic_bool cmcSuccess;
+
+
223 std::vector<double> qMin, qMax;
+
224 std::vector<double> qdotMin, qdotMax;
+
225 std::vector<double> qRefSpeeds;
+
+
+
+
+
+
Contains roboticslab::ICartesianControl and related vocabs.
+
Contains roboticslab::ICartesianSolver.
+
Definition: BasicCartesianControl.hpp:145
+
The BasicCartesianControl class implements ICartesianControl.
Definition: BasicCartesianControl.hpp:110
+
bool act(int command) override
Actuate tool.
Definition: ICartesianControlImpl.cpp:421
+
bool movl(const std::vector< double > &xd) override
Linear move to target position.
Definition: ICartesianControlImpl.cpp:175
+
void wrench(const std::vector< double > &w) override
Exert force.
Definition: ICartesianControlImpl.cpp:515
+
std::vector< std::unique_ptr< KDL::Trajectory > > trajectories
Definition: BasicCartesianControl.hpp:215
+
bool tool(const std::vector< double > &x) override
Change tool.
Definition: ICartesianControlImpl.cpp:402
+
bool wait(double timeout) override
Wait until completion.
Definition: ICartesianControlImpl.cpp:373
+
std::vector< double > fd
Definition: BasicCartesianControl.hpp:218
+
double movementStartTime
Definition: BasicCartesianControl.hpp:212
+
bool stopControl() override
Stop control.
Definition: ICartesianControlImpl.cpp:348
+
bool getParameters(std::map< int, double > ¶ms) override
Retrieve multiple configuration parameters.
Definition: ICartesianControlImpl.cpp:693
+
bool getParameter(int vocab, double *value) override
Retrieve a configuration parameter.
Definition: ICartesianControlImpl.cpp:641
+
std::vector< double > vmoStored
Definition: BasicCartesianControl.hpp:209
+
void twist(const std::vector< double > &xdot) override
Instantaneous velocity steps.
Definition: ICartesianControlImpl.cpp:475
+
bool movj(const std::vector< double > &xd) override
Move in joint space, absolute coordinates.
Definition: ICartesianControlImpl.cpp:89
+
void pose(const std::vector< double > &x) override
Achieve pose.
Definition: ICartesianControlImpl.cpp:429
+
bool stat(std::vector< double > &x, int *state=nullptr, double *timestamp=nullptr) override
Current state and position.
Definition: ICartesianControlImpl.cpp:37
+
bool forc(const std::vector< double > &fd) override
Force control.
Definition: ICartesianControlImpl.cpp:320
+
bool movv(const std::vector< double > &xdotd) override
Linear move with given velocity.
Definition: ICartesianControlImpl.cpp:251
+
bool gcmp() override
Gravity compensation.
Definition: ICartesianControlImpl.cpp:306
+
bool relj(const std::vector< double > &xd) override
Move in joint space, relative coordinates.
Definition: ICartesianControlImpl.cpp:150
+
bool setParameters(const std::map< int, double > ¶ms) override
Set multiple configuration parameters.
Definition: ICartesianControlImpl.cpp:673
+
bool setParameter(int vocab, double value) override
Set a configuration parameter.
Definition: ICartesianControlImpl.cpp:573
+
bool inv(const std::vector< double > &xd, std::vector< double > &q) override
Inverse kinematics.
Definition: ICartesianControlImpl.cpp:68
+
Abstract base class for a cartesian controller.
Definition: ICartesianControl.h:146
+
Abstract base class for a cartesian solver.
Definition: ICartesianSolver.h:23
+
reference_frame
Lists supported reference frames.
Definition: ICartesianSolver.h:27
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/CartesianControlClient_8hpp_source.html b/CartesianControlClient_8hpp_source.html
new file mode 100644
index 000000000..1d630e80d
--- /dev/null
+++ b/CartesianControlClient_8hpp_source.html
@@ -0,0 +1,192 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __CARTESIAN_CONTROL_CLIENT_HPP__
+
4 #define __CARTESIAN_CONTROL_CLIENT_HPP__
+
+
+
+
8 #include <yarp/os/Bottle.h>
+
9 #include <yarp/os/BufferedPort.h>
+
10 #include <yarp/os/PortReaderBuffer.h>
+
11 #include <yarp/os/RpcClient.h>
+
+
13 #include <yarp/dev/Drivers.h>
+
+
+
+
+
+
+
+
+
+
+
+
+
37 void onRead(yarp::os::Bottle& b)
override ;
+
38 bool getLastStatData(std::vector<double> &x,
int *state,
double * timestamp,
double timeout);
+
+
+
41 double localArrivalTime;
+
+
+
44 std::vector<double> x;
+
45 mutable std::mutex mtx;
+
+
+
+
+
+
+
+
57 bool stat (std::vector<double> &x,
int * state =
nullptr ,
double * timestamp =
nullptr )
override ;
+
58 bool inv (
const std::vector<double> &xd, std::vector<double> &q)
override ;
+
59 bool movj (
const std::vector<double> &xd)
override ;
+
60 bool relj (
const std::vector<double> &xd)
override ;
+
61 bool movl (
const std::vector<double> &xd)
override ;
+
62 bool movv (
const std::vector<double> &xdotd)
override ;
+
+
64 bool forc (
const std::vector<double> &fd)
override ;
+
+
66 bool wait (
double timeout)
override ;
+
67 bool tool (
const std::vector<double> &x)
override ;
+
68 bool act (
int command)
override ;
+
69 void pose (
const std::vector<double> &x)
override ;
+
70 void twist (
const std::vector<double> &xdot)
override ;
+
71 void wrench (
const std::vector<double> &w)
override ;
+
+
+
74 bool setParameters (
const std::map<int, double> & params)
override ;
+
+
+
+
78 bool open(yarp::os::Searchable& config)
override ;
+
79 bool close()
override ;
+
+
+
82 bool handleRpcRunnableCmd(
int vocab);
+
83 bool handleRpcConsumerCmd(
int vocab,
const std::vector<double>& in);
+
84 bool handleRpcFunctionCmd(
int vocab,
const std::vector<double>& in, std::vector<double>& out);
+
+
86 void handleStreamingConsumerCmd(
int vocab,
const std::vector<double>& in);
+
87 void handleStreamingBiConsumerCmd(
int vocab,
const std::vector<double>& in1,
double in2);
+
+
89 yarp::os::RpcClient rpcClient;
+
90 yarp::os::BufferedPort<yarp::os::Bottle> fkInPort, commandPort;
+
+
+
93 double fkStreamTimeoutSecs;
+
+
+
+
+
+
Contains roboticslab::ICartesianControl and related vocabs.
+
The CartesianControlClient class implements ICartesianControl client side.
Definition: CartesianControlClient.hpp:54
+
bool stopControl() override
Stop control.
Definition: ICartesianControlImpl.cpp:237
+
bool forc(const std::vector< double > &fd) override
Force control.
Definition: ICartesianControlImpl.cpp:230
+
bool tool(const std::vector< double > &x) override
Change tool.
Definition: ICartesianControlImpl.cpp:258
+
void pose(const std::vector< double > &x) override
Achieve pose.
Definition: ICartesianControlImpl.cpp:279
+
bool movl(const std::vector< double > &xd) override
Linear move to target position.
Definition: ICartesianControlImpl.cpp:209
+
bool inv(const std::vector< double > &xd, std::vector< double > &q) override
Inverse kinematics.
Definition: ICartesianControlImpl.cpp:188
+
bool stat(std::vector< double > &x, int *state=nullptr, double *timestamp=nullptr) override
Current state and position.
Definition: ICartesianControlImpl.cpp:141
+
void twist(const std::vector< double > &xdot) override
Instantaneous velocity steps.
Definition: ICartesianControlImpl.cpp:286
+
bool gcmp() override
Gravity compensation.
Definition: ICartesianControlImpl.cpp:223
+
bool setParameter(int vocab, double value) override
Set a configuration parameter.
Definition: ICartesianControlImpl.cpp:300
+
bool relj(const std::vector< double > &xd) override
Move in joint space, relative coordinates.
Definition: ICartesianControlImpl.cpp:202
+
void wrench(const std::vector< double > &w) override
Exert force.
Definition: ICartesianControlImpl.cpp:293
+
bool getParameter(int vocab, double *value) override
Retrieve a configuration parameter.
Definition: ICartesianControlImpl.cpp:315
+
bool movv(const std::vector< double > &xdotd) override
Linear move with given velocity.
Definition: ICartesianControlImpl.cpp:216
+
bool movj(const std::vector< double > &xd) override
Move in joint space, absolute coordinates.
Definition: ICartesianControlImpl.cpp:195
+
bool setParameters(const std::map< int, double > ¶ms) override
Set multiple configuration parameters.
Definition: ICartesianControlImpl.cpp:336
+
bool getParameters(std::map< int, double > ¶ms) override
Retrieve multiple configuration parameters.
Definition: ICartesianControlImpl.cpp:357
+
bool act(int command) override
Actuate tool.
Definition: ICartesianControlImpl.cpp:265
+
bool wait(double timeout) override
Wait until completion.
Definition: ICartesianControlImpl.cpp:244
+
Responds to streaming FK messages.
Definition: CartesianControlClient.hpp:34
+
Abstract base class for a cartesian controller.
Definition: ICartesianControl.h:146
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/CartesianControlServer_8hpp_source.html b/CartesianControlServer_8hpp_source.html
new file mode 100644
index 000000000..e4bd0ccb2
--- /dev/null
+++ b/CartesianControlServer_8hpp_source.html
@@ -0,0 +1,239 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __CARTESIAN_CONTROL_SERVER_HPP__
+
4 #define __CARTESIAN_CONTROL_SERVER_HPP__
+
+
+
+
8 #include <yarp/os/Bottle.h>
+
9 #include <yarp/os/BufferedPort.h>
+
10 #include <yarp/os/PeriodicThread.h>
+
11 #include <yarp/os/RpcServer.h>
+
+
13 #include <yarp/dev/Drivers.h>
+
14 #include <yarp/dev/PolyDriver.h>
+
+
+
17 #include "KinematicRepresentation.hpp"
+
+
+
+
+
+
30 class RpcTransformResponder;
+
31 class StreamResponder;
+
+
+
38 public yarp::os::PeriodicThread
+
+
+
+
+
+
+
+
46 bool open(yarp::os::Searchable & config)
override ;
+
47 bool close()
override ;
+
+
+
+
+
+
53 yarp::dev::PolyDriver cartesianControlDevice;
+
+
55 yarp::os::RpcServer rpcServer, rpcTransformServer;
+
56 yarp::os::BufferedPort<yarp::os::Bottle> fkOutPort, commandPort;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
75 : iCartesianControl(_iCartesianControl)
+
+
+
+
+
+
87 bool respond (
const yarp::os::Bottle & in, yarp::os::Bottle & out)
override ;
+
+
+
+
+
95 virtual bool transformIncomingData(std::vector<double> & vin)
+
+
+
98 virtual bool transformOutgoingData(std::vector<double> & vout)
+
+
+
+
+
+
104 using FunctionFun = bool (
ICartesianControl ::*)(
const std::vector<double> &, std::vector<double> &);
+
+
106 bool handleStatMsg(
const yarp::os::Bottle & in, yarp::os::Bottle & out);
+
107 bool handleWaitMsg(
const yarp::os::Bottle & in, yarp::os::Bottle & out);
+
108 bool handleActMsg(
const yarp::os::Bottle & in, yarp::os::Bottle & out);
+
+
110 bool handleRunnableCmdMsg(
const yarp::os::Bottle & in, yarp::os::Bottle & out, RunnableFun cmd);
+
111 bool handleConsumerCmdMsg(
const yarp::os::Bottle & in, yarp::os::Bottle & out, ConsumerFun cmd);
+
112 bool handleFunctionCmdMsg(
const yarp::os::Bottle & in, yarp::os::Bottle & out, FunctionFun cmd);
+
+
114 bool handleParameterSetter(
const yarp::os::Bottle & in, yarp::os::Bottle & out);
+
115 bool handleParameterGetter(
const yarp::os::Bottle & in, yarp::os::Bottle & out);
+
+
117 bool handleParameterSetterGroup(
const yarp::os::Bottle & in, yarp::os::Bottle & out);
+
118 bool handleParameterGetterGroup(
const yarp::os::Bottle & in, yarp::os::Bottle & out);
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
141 bool transformIncomingData(std::vector<double> & vin)
override ;
+
142 bool transformOutgoingData(std::vector<double> & vout)
override ;
+
+
+
+
+
+
+
+
+
+
+
157 : iCartesianControl(_iCartesianControl)
+
+
+
160 void onRead(yarp::os::Bottle & b)
override ;
+
+
+
+
164 using BiConsumerFun = void (
ICartesianControl ::*)(
const std::vector<double> &, double);
+
+
166 void handleConsumerCmdMsg(
const yarp::os::Bottle & in, ConsumerFun cmd);
+
167 void handleBiConsumerCmdMsg(
const yarp::os::Bottle & in, BiConsumerFun cmd);
+
+
+
+
+
+
+
+
Contains roboticslab::ICartesianControl and related vocabs.
+
The CartesianControlServer class implements ICartesianControl server side.
Definition: CartesianControlServer.hpp:39
+
Abstract base class for a cartesian controller.
Definition: ICartesianControl.h:146
+
Responds to RPC command messages.
Definition: CartesianControlServer.hpp:72
+
void makeUsage()
Definition: RpcResponder.cpp:91
+
bool respond(const yarp::os::Bottle &in, yarp::os::Bottle &out) override
Definition: RpcResponder.cpp:52
+
+
Responds to streaming command messages.
Definition: CartesianControlServer.hpp:154
+
coordinate_system
Lists available translational representations.
Definition: KinematicRepresentation.hpp:27
+
orientation_system
Lists available rotational representations.
Definition: KinematicRepresentation.hpp:36
+
angular_units
Lists recognized angular units.
Definition: KinematicRepresentation.hpp:48
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/CentroidTransform_8hpp_source.html b/CentroidTransform_8hpp_source.html
new file mode 100644
index 000000000..8b9919311
--- /dev/null
+++ b/CentroidTransform_8hpp_source.html
@@ -0,0 +1,137 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/streamingDeviceController/CentroidTransform.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
1 #ifndef __CENTROID_TRANSFORM_HPP__
+
2 #define __CENTROID_TRANSFORM_HPP__
+
+
4 #include <yarp/os/Bottle.h>
+
5 #include <yarp/os/Stamp.h>
+
+
7 #include <kdl/frames.hpp>
+
+
9 #include "StreamingDevice.hpp"
+
+
+
+
+
14 class StreamingDevice;
+
+
+
+
+
+
+
+
29 { this->streamingDevice = streamingDevice; }
+
+
+
+
+
36 { this->permanenceTime = permanenceTime; }
+
+
+
+
+
+
+
+
46 double permanenceTime;
+
47 yarp::os::Bottle lastBottle;
+
48 yarp::os::Stamp lastAcquisition;
+
49 KDL::Rotation rot_tcp_camera;
+
+
+
+
+
+
+
+
+
+
+
+
+
Abstract class for a YARP streaming device.
Definition: StreamingDevice.hpp:46
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/ChainFkSolverPos__ST_8hpp_source.html b/ChainFkSolverPos__ST_8hpp_source.html
new file mode 100644
index 000000000..57be2b40a
--- /dev/null
+++ b/ChainFkSolverPos__ST_8hpp_source.html
@@ -0,0 +1,134 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/KdlSolver/ChainFkSolverPos_ST.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __CHAIN_FK_SOLVER_POS_ST_HPP__
+
4 #define __CHAIN_FK_SOLVER_POS_ST_HPP__
+
+
6 #include <kdl/chainfksolver.hpp>
+
+
8 #include "ProductOfExponentials.hpp"
+
+
+
+
+
+
+
+
33 int JntToCart (
const KDL::JntArray & q_in, KDL::Frame & p_out,
int segmentNr = -1)
override ;
+
+
46 int JntToCart (
const KDL::JntArray & q_in, std::vector<KDL::Frame> & p_out,
int segmentNr = -1)
override ;
+
+
+
+
65 const char *
strError (
const int error)
const override ;
+
+
74 static KDL::ChainFkSolverPos *
create (
const KDL::Chain & chain);
+
+
+
+
+
+
+
+
+
85 const KDL::Chain & chain;
+
+
+
+
+
+
+
+
FK solver using Screw Theory.
Definition: ChainFkSolverPos_ST.hpp:22
+
void updateInternalDataStructures() override
Update the internal data structures.
Definition: ChainFkSolverPos_ST.cpp:40
+
static const int E_ILLEGAL_ARGUMENT_SIZE
Return code, input vector size does not match expected output vector size.
Definition: ChainFkSolverPos_ST.hpp:80
+
static const int E_OPERATION_NOT_SUPPORTED
Return code, operation not supported.
Definition: ChainFkSolverPos_ST.hpp:77
+
static KDL::ChainFkSolverPos * create(const KDL::Chain &chain)
Create an instance of ChainFkSolverPos_ST.
Definition: ChainFkSolverPos_ST.cpp:62
+
const char * strError(const int error) const override
Return a description of the last error.
Definition: ChainFkSolverPos_ST.cpp:47
+
int JntToCart(const KDL::JntArray &q_in, KDL::Frame &p_out, int segmentNr=-1) override
Perform FK on the selected segment.
Definition: ChainFkSolverPos_ST.cpp:16
+
Abstraction of a product of exponentials (POE) formula.
Definition: ProductOfExponentials.hpp:28
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/ChainIkSolverPos__ID_8hpp_source.html b/ChainIkSolverPos__ID_8hpp_source.html
new file mode 100644
index 000000000..ca6f60f4f
--- /dev/null
+++ b/ChainIkSolverPos__ID_8hpp_source.html
@@ -0,0 +1,141 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/KdlSolver/ChainIkSolverPos_ID.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __CHAIN_IK_SOLVER_POS_ID_HPP__
+
4 #define __CHAIN_IK_SOLVER_POS_ID_HPP__
+
+
6 #include <kdl/chain.hpp>
+
7 #include <kdl/chainfksolver.hpp>
+
8 #include <kdl/chainiksolver.hpp>
+
9 #include <kdl/chainjnttojacsolver.hpp>
+
10 #include <kdl/jacobian.hpp>
+
11 #include <kdl/jntarray.hpp>
+
+
+
+
+
+
+
+
36 ChainIkSolverPos_ID (
const KDL::Chain & chain,
const KDL::JntArray & q_min,
const KDL::JntArray & q_max, KDL::ChainFkSolverPos & fksolver);
+
+
48 int CartToJnt (
const KDL::JntArray & q_init,
const KDL::Frame & p_in, KDL::JntArray & q_out)
override ;
+
+
+
+
67 const char *
strError (
const int error)
const override ;
+
+
+
+
+
+
+
76 KDL::JntArray computeDiffInvKin(
const KDL::Twist & delta_twist);
+
+
78 const KDL::Chain & chain;
+
+
+
+
+
+
84 KDL::ChainFkSolverPos & fkSolverPos;
+
85 KDL::ChainJntToJacSolver jacSolver;
+
+
87 KDL::Jacobian jacobian;
+
+
+
+
+
+
IK solver using infinitesimal displacement twists.
Definition: ChainIkSolverPos_ID.hpp:25
+
void updateInternalDataStructures() override
Update the internal data structures.
Definition: ChainIkSolverPos_ID.cpp:107
+
static const int E_FKSOLVERPOS_FAILED
Return code, internal FK position solver failed.
Definition: ChainIkSolverPos_ID.hpp:70
+
static const int E_JACSOLVER_FAILED
Return code, internal Jacobian solver failed.
Definition: ChainIkSolverPos_ID.hpp:73
+
ChainIkSolverPos_ID(const KDL::Chain &chain, const KDL::JntArray &q_min, const KDL::JntArray &q_max, KDL::ChainFkSolverPos &fksolver)
Constructor.
Definition: ChainIkSolverPos_ID.cpp:16
+
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out) override
Calculate inverse position kinematics.
Definition: ChainIkSolverPos_ID.cpp:29
+
const char * strError(const int error) const override
Return a description of the last error.
Definition: ChainIkSolverPos_ID.cpp:119
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/ChainIkSolverPos__ST_8hpp_source.html b/ChainIkSolverPos__ST_8hpp_source.html
new file mode 100644
index 000000000..2550392bd
--- /dev/null
+++ b/ChainIkSolverPos__ST_8hpp_source.html
@@ -0,0 +1,143 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/KdlSolver/ChainIkSolverPos_ST.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __CHAIN_IK_SOLVER_POS_ST_HPP__
+
4 #define __CHAIN_IK_SOLVER_POS_ST_HPP__
+
+
6 #include <kdl/chainiksolver.hpp>
+
+
8 #include "ScrewTheoryIkProblem.hpp"
+
9 #include "ConfigurationSelector.hpp"
+
+
+
+
+
+
+
+
+
+
39 int CartToJnt (
const KDL::JntArray & q_init,
const KDL::Frame & p_in, KDL::JntArray & q_out)
override ;
+
+
+
+
58 const char *
strError (
const int error)
const override ;
+
+
+
+
+
+
+
+
+
+
+
+
+
83 const KDL::Chain & chain;
+
+
+
+
+
+
+
+
+
+
IK solver using Screw Theory.
Definition: ChainIkSolverPos_ST.hpp:24
+
static KDL::ChainIkSolverPos * create(const KDL::Chain &chain, const ConfigurationSelectorFactory &configFactory)
Create an instance of ChainIkSolverPos_ST.
Definition: ChainIkSolverPos_ST.cpp:82
+
static const int E_SOLUTION_NOT_FOUND
Return code, IK solution not found.
Definition: ChainIkSolverPos_ST.hpp:72
+
void updateInternalDataStructures() override
Update the internal data structures.
Definition: ChainIkSolverPos_ST.cpp:64
+
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out) override
Calculate inverse position kinematics.
Definition: ChainIkSolverPos_ST.cpp:36
+
static const int E_OUT_OF_LIMITS
Return code, target pose out of robot limits.
Definition: ChainIkSolverPos_ST.hpp:75
+
virtual ~ChainIkSolverPos_ST()
Destructor.
Definition: ChainIkSolverPos_ST.cpp:25
+
static const int E_NOT_REACHABLE
Return code, solution out of reach.
Definition: ChainIkSolverPos_ST.hpp:78
+
const char * strError(const int error) const override
Return a description of the last error.
Definition: ChainIkSolverPos_ST.cpp:106
+
Base factory class for ConfigurationSelector.
Definition: ConfigurationSelector.hpp:183
+
Abstract base class for a robot configuration strategy selector.
Definition: ConfigurationSelector.hpp:20
+
Proxy IK problem solver class that iterates over a sequence of subproblems.
Definition: ScrewTheoryIkProblem.hpp:95
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/ConfigurationSelector_8hpp_source.html b/ConfigurationSelector_8hpp_source.html
new file mode 100644
index 000000000..372402888
--- /dev/null
+++ b/ConfigurationSelector_8hpp_source.html
@@ -0,0 +1,264 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/ScrewTheoryLib/ConfigurationSelector.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __CONFIGURATION_SELECTOR_HPP__
+
4 #define __CONFIGURATION_SELECTOR_HPP__
+
+
+
+
8 #include <kdl/frames.hpp>
+
9 #include <kdl/jntarray.hpp>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
44 virtual bool configure (
const std::vector<KDL::JntArray> & solutions);
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
79 void store (
const KDL::JntArray * q)
+
+
+
83 void validate (
const KDL::JntArray & qMin,
const KDL::JntArray & qMax);
+
+
+
+
+
+
+
+
+
+
+
+
98 const KDL::JntArray * q;
+
+
+
+
102 KDL::JntArray _qMin, _qMax;
+
+
104 Configuration optimalConfig;
+
+
106 std::vector<Configuration> configs;
+
+
+
+
+
+
+
+
132 lastValid(INVALID_CONFIG)
+
+
+
+
+
+
139 std::vector<double>
getDiffs (
const KDL::JntArray & qGuess,
const Configuration & config);
+
+
+
+
143 static const int INVALID_CONFIG = -1;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
201 : _qMin(qMin), _qMax(qMax)
+
+
+
204 KDL::JntArray _qMin, _qMax;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
255 if (_qMin.rows() == 6 && _qMax.rows() == 6)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Base factory class for ConfigurationSelector.
Definition: ConfigurationSelector.hpp:183
+
ConfigurationSelectorFactory(const KDL::JntArray &qMin, const KDL::JntArray &qMax)
Constructor.
Definition: ConfigurationSelector.hpp:200
+
virtual ConfigurationSelector * create() const =0
Creates an instance of the concrete class.
+
Implementation factory class for ConfigurationSelectorHumanoidGait.
Definition: ConfigurationSelector.hpp:241
+
ConfigurationSelectorHumanoidGaitFactory(const KDL::JntArray &qMin, const KDL::JntArray &qMax)
Constructor.
Definition: ConfigurationSelector.hpp:249
+
ConfigurationSelector * create() const override
Creates an instance of the concrete class.
Definition: ConfigurationSelector.hpp:253
+
IK solver configuration strategy selector based on human walking.
Definition: ConfigurationSelector.hpp:155
+
bool applyConstraints(const Configuration &config)
Determines whether the configuration is valid according to this selector's premises.
Definition: ConfigurationSelectorHumanoidGait.cpp:29
+
bool findOptimalConfiguration(const KDL::JntArray &qGuess) override
Analyzes available configurations and selects the optimal one.
Definition: ConfigurationSelectorHumanoidGait.cpp:11
+
ConfigurationSelectorHumanoidGait(const KDL::JntArray &qMin, const KDL::JntArray &qMax)
Constructor.
Definition: ConfigurationSelector.hpp:163
+
Implementation factory class for ConfigurationSelectorLeastOverallAngularDisplacement.
Definition: ConfigurationSelector.hpp:215
+
ConfigurationSelector * create() const override
Creates an instance of the concrete class.
Definition: ConfigurationSelector.hpp:227
+
ConfigurationSelectorLeastOverallAngularDisplacementFactory(const KDL::JntArray &qMin, const KDL::JntArray &qMax)
Constructor.
Definition: ConfigurationSelector.hpp:223
+
IK solver configuration strategy selector based on the overall displacement of all joints.
Definition: ConfigurationSelector.hpp:122
+
ConfigurationSelectorLeastOverallAngularDisplacement(const KDL::JntArray &qMin, const KDL::JntArray &qMax)
Constructor.
Definition: ConfigurationSelector.hpp:130
+
std::vector< double > getDiffs(const KDL::JntArray &qGuess, const Configuration &config)
Obtains vector of differences between current and desired joint values.
Definition: ConfigurationSelectorLeastOverallAngularDisplacement.cpp:58
+
bool findOptimalConfiguration(const KDL::JntArray &qGuess) override
Analyzes available configurations and selects the optimal one.
Definition: ConfigurationSelectorLeastOverallAngularDisplacement.cpp:12
+
Helper class to store a specific robot configuration.
Definition: ConfigurationSelector.hpp:70
+
const KDL::JntArray * retrievePose() const
Retrieves stored joint values.
Definition: ConfigurationSelector.hpp:86
+
Configuration()
Constructor.
Definition: ConfigurationSelector.hpp:73
+
void validate(const KDL::JntArray &qMin, const KDL::JntArray &qMax)
Checks reachability against provided joint limits.
Definition: ConfigurationSelector.cpp:39
+
void store(const KDL::JntArray *q)
Initializes joint values.
Definition: ConfigurationSelector.hpp:79
+
bool isValid() const
Whether this configuration is attainable or not.
Definition: ConfigurationSelector.hpp:90
+
void invalidate()
Mark this configuration as invalid.
Definition: ConfigurationSelector.hpp:94
+
Abstract base class for a robot configuration strategy selector.
Definition: ConfigurationSelector.hpp:20
+
ConfigurationSelector(const KDL::JntArray &qMin, const KDL::JntArray &qMax)
Constructor.
Definition: ConfigurationSelector.hpp:28
+
virtual bool findOptimalConfiguration(const KDL::JntArray &qGuess)=0
Analyzes available configurations and selects the optimal one.
+
virtual ~ConfigurationSelector()=default
Destructor.
+
virtual bool configure(const std::vector< KDL::JntArray > &solutions)
Stores initial values for a specific pose.
Definition: ConfigurationSelector.cpp:21
+
virtual void retrievePose(KDL::JntArray &q) const
Queries computed joint values for the optimal configuration.
Definition: ConfigurationSelector.hpp:60
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/FtCompensation_8hpp_source.html b/FtCompensation_8hpp_source.html
new file mode 100644
index 000000000..4914bb325
--- /dev/null
+++ b/FtCompensation_8hpp_source.html
@@ -0,0 +1,169 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/ftCompensation/FtCompensation.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __FT_COMPENSATION_HPP__
+
4 #define __FT_COMPENSATION_HPP__
+
+
6 #include <yarp/os/PeriodicThread.h>
+
7 #include <yarp/os/RFModule.h>
+
+
9 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
+
10 #include <yarp/dev/PolyDriver.h>
+
+
12 #include <kdl/frames.hpp>
+
+
+
+
+
+
+
+
27 public yarp::os::PeriodicThread
+
+
+
+
31 : yarp::os::PeriodicThread(1.0, yarp::os::ShouldUseSystemClock::Yes, yarp::os::PeriodicThreadClock::Absolute)
+
+
+
+
+
+
37 bool configure(yarp::os::ResourceFinder & rf)
override ;
+
38 bool updateModule()
override ;
+
39 bool interruptModule()
override ;
+
40 double getPeriod()
override ;
+
41 bool close()
override ;
+
+
+
+
+
+
47 bool readSensor(KDL::Wrench & wrench)
const ;
+
48 bool compensateTool(KDL::Wrench & wrench)
const ;
+
49 bool applyImpedance(KDL::Wrench & wrench);
+
+
51 yarp::dev::PolyDriver cartesianDevice;
+
+
+
+
55 yarp::dev::PolyDriver sensorDevice;
+
56 yarp::dev::ISixAxisForceTorqueSensors * sensor;
+
+
58 KDL::Rotation R_N_sensor;
+
59 KDL::Vector toolCoM_N;
+
60 KDL::Wrench toolWeight_0;
+
61 KDL::Wrench initialOffset;
+
62 KDL::Frame initialPose;
+
63 KDL::Frame previousPose;
+
+
+
66 cartesian_cmd command;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
82 double torqueDeadband;
+
+
+
+
+
+
Contains roboticslab::ICartesianControl and related vocabs.
+
Produces motion in the direction of an externally applied force measured by a force-torque sensor (pr...
Definition: FtCompensation.hpp:28
+
Abstract base class for a cartesian controller.
Definition: ICartesianControl.h:146
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/GrabberResponder_8hpp_source.html b/GrabberResponder_8hpp_source.html
new file mode 100644
index 000000000..3dbdb2cf4
--- /dev/null
+++ b/GrabberResponder_8hpp_source.html
@@ -0,0 +1,131 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/haarDetectionController/GrabberResponder.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __GRABBER_RESPONDER_HPP__
+
4 #define __GRABBER_RESPONDER_HPP__
+
+
6 #include <yarp/os/Bottle.h>
+
7 #include <yarp/os/PortReaderBuffer.h>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
28 void onRead(yarp::os::Bottle &b)
override ;
+
+
+
+
32 this->iCartesianControl = iCartesianControl;
+
+
+
35 void setNoApproachSetting(
bool noApproach)
+
+
37 this->noApproach = noApproach;
+
+
+
+
+
+
43 bool isStopped, noApproach;
+
+
+
+
+
+
Contains roboticslab::ICartesianControl and related vocabs.
+
Callback class for dealing with incoming grabber data streams.
Definition: GrabberResponder.hpp:21
+
Abstract base class for a cartesian controller.
Definition: ICartesianControl.h:146
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/HaarDetectionController_8hpp_source.html b/HaarDetectionController_8hpp_source.html
new file mode 100644
index 000000000..e54d102c3
--- /dev/null
+++ b/HaarDetectionController_8hpp_source.html
@@ -0,0 +1,136 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/haarDetectionController/HaarDetectionController.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __HAAR_DETECTION_CONTROLLER_HPP__
+
4 #define __HAAR_DETECTION_CONTROLLER_HPP__
+
+
6 #include <yarp/os/BufferedPort.h>
+
7 #include <yarp/os/RFModule.h>
+
+
9 #include <yarp/dev/PolyDriver.h>
+
+
+
12 #include "IProximitySensors.h"
+
+
14 #include "GrabberResponder.hpp"
+
+
+
+
+
+
+
+
+
+
+
31 bool configure(yarp::os::ResourceFinder & rf)
override ;
+
32 bool updateModule()
override ;
+
33 bool interruptModule()
override ;
+
34 bool close()
override ;
+
35 double getPeriod()
override ;
+
+
+
+
39 yarp::os::BufferedPort<yarp::os::Bottle> grabberPort;
+
+
41 yarp::dev::PolyDriver cartesianControlDevice;
+
+
+
44 yarp::dev::PolyDriver sensorsClientDevice;
+
45 roboticslab::IProximitySensors * iProximitySensors;
+
+
+
+
+
+
+
+
Contains roboticslab::ICartesianControl and related vocabs.
+
Callback class for dealing with incoming grabber data streams.
Definition: GrabberResponder.hpp:21
+
Create seek-and-follow trajectories based on Haar detection algorithms.
Definition: HaarDetectionController.hpp:26
+
Abstract base class for a cartesian controller.
Definition: ICartesianControl.h:146
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/ICartesianControl_8h.html b/ICartesianControl_8h.html
new file mode 100644
index 000000000..6482fe109
--- /dev/null
+++ b/ICartesianControl_8h.html
@@ -0,0 +1,287 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/ICartesianControl.h File Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Contains roboticslab::ICartesianControl and related vocabs.
+More...
+
#include <map>
+
#include <vector>
+
#include <yarp/os/Vocab.h>
+
#include <ICartesianSolver.h >
+
+
Go to the source code of this file.
+
+
+ roboticslab
+ The main, catch-all namespace for Robotics Lab UC3M.
+
+
+
+
+Used in acknowledge responses, configuration accessors , etc..
+
+
+constexpr int VOCAB_CC_OK = yarp::os::createVocab32('o','k')
+ Success.
+
+
+constexpr int VOCAB_CC_FAILED = yarp::os::createVocab32('f','a','i','l')
+ Failure.
+
+
+constexpr int VOCAB_CC_SET = yarp::os::createVocab32('s','e','t')
+ Setter.
+
+
+constexpr int VOCAB_CC_GET = yarp::os::createVocab32('g','e','t')
+ Getter.
+
+
+constexpr int VOCAB_CC_NOT_SET = yarp::os::createVocab32('n','s','e','t')
+ State: not set.
+
+
+
+
+constexpr int VOCAB_CC_STAT = yarp::os::createVocab32('s','t','a','t')
+ Current state and position.
+
+
+constexpr int VOCAB_CC_INV = yarp::os::createVocab32('i','n','v')
+ Inverse kinematics.
+
+
+constexpr int VOCAB_CC_MOVJ = yarp::os::createVocab32('m','o','v','j')
+ Move in joint space, absolute coordinates.
+
+
+constexpr int VOCAB_CC_RELJ = yarp::os::createVocab32('r','e','l','j')
+ Move in joint space, relative coordinates.
+
+
+constexpr int VOCAB_CC_MOVL = yarp::os::createVocab32('m','o','v','l')
+ Linear move to target position.
+
+
+constexpr int VOCAB_CC_MOVV = yarp::os::createVocab32('m','o','v','v')
+ Linear move with given velocity.
+
+
+constexpr int VOCAB_CC_GCMP = yarp::os::createVocab32('g','c','m','p')
+ Gravity compensation.
+
+
+constexpr int VOCAB_CC_FORC = yarp::os::createVocab32('f','o','r','c')
+ Force control.
+
+
+constexpr int VOCAB_CC_STOP = yarp::os::createVocab32('s','t','o','p')
+ Stop control.
+
+
+constexpr int VOCAB_CC_WAIT = yarp::os::createVocab32('w','a','i','t')
+ Wait motion done.
+
+
+constexpr int VOCAB_CC_TOOL = yarp::os::createVocab32('t','o','o','l')
+ Change tool.
+
+
+constexpr int VOCAB_CC_ACT = yarp::os::createVocab32('a','c','t')
+ Actuate tool.
+
+
+
+
+constexpr int VOCAB_CC_POSE = yarp::os::createVocab32('p','o','s','e')
+ Achieve pose.
+
+
+constexpr int VOCAB_CC_MOVI = yarp::os::createVocab32('m','o','v','i')
+
+
+constexpr int VOCAB_CC_TWIST = yarp::os::createVocab32('t','w','s','t')
+ Instantaneous velocity steps.
+
+
+constexpr int VOCAB_CC_WRENCH = yarp::os::createVocab32('w','r','n','c')
+ Exert force.
+
+
+Used by roboticslab::ICartesianControl::stat to reflect current control state.
+
+
+constexpr int VOCAB_CC_NOT_CONTROLLING = yarp::os::createVocab32('c','c','n','c')
+ Not controlling.
+
+
+constexpr int VOCAB_CC_MOVJ_CONTROLLING = yarp::os::createVocab32('c','c','j','c')
+ Controlling MOVJ commands.
+
+
+constexpr int VOCAB_CC_MOVL_CONTROLLING = yarp::os::createVocab32('c','c','l','c')
+ Controlling MOVL commands.
+
+
+constexpr int VOCAB_CC_MOVV_CONTROLLING = yarp::os::createVocab32('c','c','v','c')
+ Controlling MOVV commands.
+
+
+constexpr int VOCAB_CC_GCMP_CONTROLLING = yarp::os::createVocab32('c','c','g','c')
+ Controlling GCMP commands.
+
+
+constexpr int VOCAB_CC_FORC_CONTROLLING = yarp::os::createVocab32('c','c','f','c')
+ Controlling FORC commands.
+
+
+
+
Used by roboticslab::ICartesianControl::act to control the actuator.
+
+
+constexpr int VOCAB_CC_ACTUATOR_NONE = yarp::os::createVocab32('a','c','n')
+ No actuator or no action.
+
+
+constexpr int VOCAB_CC_ACTUATOR_CLOSE_GRIPPER = yarp::os::createVocab32('a','c','c','g')
+ Close gripper.
+
+
+constexpr int VOCAB_CC_ACTUATOR_OPEN_GRIPPER = yarp::os::createVocab32('a','c','o','g')
+ Open gripper.
+
+
+constexpr int VOCAB_CC_ACTUATOR_STOP_GRIPPER = yarp::os::createVocab32('a','c','s','g')
+ Stop gripper.
+
+
+constexpr int VOCAB_CC_ACTUATOR_GENERIC = yarp::os::createVocab32('a','c','g')
+ Generic actuator.
+
+
+
+
+constexpr int VOCAB_CC_CONFIG_PARAMS = yarp::os::createVocab32('p','r','m','s')
+ Parameter group.
+
+
+constexpr int VOCAB_CC_CONFIG_GAIN = yarp::os::createVocab32('c','p','c','g')
+ Controller gain.
+
+
+constexpr int VOCAB_CC_CONFIG_TRAJ_DURATION = yarp::os::createVocab32('c','p','t','d')
+ Trajectory duration.
+
+
+constexpr int VOCAB_CC_CONFIG_CMC_PERIOD = yarp::os::createVocab32('c','p','c','p')
+ CMC period [ms].
+
+
+constexpr int VOCAB_CC_CONFIG_WAIT_PERIOD = yarp::os::createVocab32('c','p','w','p')
+ Check period of 'wait' command [ms].
+
+
+constexpr int VOCAB_CC_CONFIG_FRAME = yarp::os::createVocab32('c','p','f')
+ Reference frame.
+
+
+constexpr int VOCAB_CC_CONFIG_STREAMING_CMD = yarp::os::createVocab32('c','p','s','c')
+ Preset streaming command.
+
+
+
+
+
+
+
diff --git a/ICartesianControl_8h_source.html b/ICartesianControl_8h_source.html
new file mode 100644
index 000000000..8b2065aac
--- /dev/null
+++ b/ICartesianControl_8h_source.html
@@ -0,0 +1,283 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/ICartesianControl.h Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Go to the documentation of this file.
+
+
3 #ifndef __I_CARTESIAN_CONTROL__
+
4 #define __I_CARTESIAN_CONTROL__
+
+
+
+
+
9 #include <yarp/os/Vocab.h>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
77 #ifndef SWIG_PREPROCESSOR_SHOULD_SKIP_THIS
+
78 [[deprecated(
"use `VOCAB_CC_POSE` instead" )]]
+
79 constexpr
int VOCAB_CC_MOVI = yarp::os::createVocab32(
'm' ,
'o' ,
'v' ,
'i' );
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
175 virtual bool stat (std::vector<double> &x,
int * state =
nullptr ,
double * timestamp =
nullptr ) = 0;
+
+
189 virtual bool inv (
const std::vector<double> &xd, std::vector<double> &q) = 0;
+
+
205 virtual bool movj (
const std::vector<double> &xd) = 0;
+
+
221 virtual bool relj (
const std::vector<double> &xd) = 0;
+
+
234 virtual bool movl (
const std::vector<double> &xd) = 0;
+
+
247 virtual bool movv (
const std::vector<double> &xdotd) = 0;
+
+
+
+
269 virtual bool forc (
const std::vector<double> &fd) = 0;
+
+
+
+
291 virtual bool wait (
double timeout = 0.0) = 0;
+
+
304 virtual bool tool (
const std::vector<double> &x) = 0;
+
+
315 virtual bool act (
int command) = 0;
+
+
+
+
340 virtual void pose (
const std::vector<double> &x) = 0;
+
+
342 #ifndef SWIG_PREPROCESSOR_SHOULD_SKIP_THIS
+
343 [[deprecated(
"use `pose` instead" )]]
+
344 virtual void movi(
const std::vector<double> &x)
+
+
+
+
+
+
359 virtual void twist (
const std::vector<double> &xdot) = 0;
+
+
369 virtual void wrench (
const std::vector<double> &w) = 0;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
constexpr int VOCAB_CC_STAT
Current state and position.
Definition: ICartesianControl.h:52
+
constexpr int VOCAB_CC_RELJ
Move in joint space, relative coordinates.
Definition: ICartesianControl.h:55
+
constexpr int VOCAB_CC_ACTUATOR_CLOSE_GRIPPER
Close gripper.
Definition: ICartesianControl.h:115
+
constexpr int VOCAB_CC_MOVJ
Move in joint space, absolute coordinates.
Definition: ICartesianControl.h:54
+
constexpr int VOCAB_CC_ACTUATOR_NONE
No actuator or no action.
Definition: ICartesianControl.h:114
+
constexpr int VOCAB_CC_TOOL
Change tool.
Definition: ICartesianControl.h:62
+
constexpr int VOCAB_CC_FORC
Force control.
Definition: ICartesianControl.h:59
+
constexpr int VOCAB_CC_FORC_CONTROLLING
Controlling FORC commands.
Definition: ICartesianControl.h:100
+
constexpr int VOCAB_CC_GCMP_CONTROLLING
Controlling GCMP commands.
Definition: ICartesianControl.h:99
+
constexpr int VOCAB_CC_CONFIG_TRAJ_DURATION
Trajectory duration.
Definition: ICartesianControl.h:131
+
constexpr int VOCAB_CC_GCMP
Gravity compensation.
Definition: ICartesianControl.h:58
+
constexpr int VOCAB_CC_SET
Setter.
Definition: ICartesianControl.h:37
+
constexpr int VOCAB_CC_CONFIG_PARAMS
Parameter group.
Definition: ICartesianControl.h:129
+
constexpr int VOCAB_CC_INV
Inverse kinematics.
Definition: ICartesianControl.h:53
+
constexpr int VOCAB_CC_CONFIG_FRAME
Reference frame.
Definition: ICartesianControl.h:134
+
constexpr int VOCAB_CC_ACTUATOR_STOP_GRIPPER
Stop gripper.
Definition: ICartesianControl.h:117
+
constexpr int VOCAB_CC_OK
Success.
Definition: ICartesianControl.h:35
+
constexpr int VOCAB_CC_GET
Getter.
Definition: ICartesianControl.h:38
+
constexpr int VOCAB_CC_WRENCH
Exert force.
Definition: ICartesianControl.h:82
+
constexpr int VOCAB_CC_NOT_SET
State: not set.
Definition: ICartesianControl.h:39
+
constexpr int VOCAB_CC_ACTUATOR_GENERIC
Generic actuator.
Definition: ICartesianControl.h:118
+
constexpr int VOCAB_CC_ACT
Actuate tool.
Definition: ICartesianControl.h:63
+
constexpr int VOCAB_CC_TWIST
Instantaneous velocity steps.
Definition: ICartesianControl.h:81
+
constexpr int VOCAB_CC_CONFIG_WAIT_PERIOD
Check period of 'wait' command [ms].
Definition: ICartesianControl.h:133
+
constexpr int VOCAB_CC_WAIT
Wait motion done.
Definition: ICartesianControl.h:61
+
constexpr int VOCAB_CC_FAILED
Failure.
Definition: ICartesianControl.h:36
+
constexpr int VOCAB_CC_NOT_CONTROLLING
Not controlling.
Definition: ICartesianControl.h:95
+
constexpr int VOCAB_CC_CONFIG_CMC_PERIOD
CMC period [ms].
Definition: ICartesianControl.h:132
+
constexpr int VOCAB_CC_MOVV_CONTROLLING
Controlling MOVV commands.
Definition: ICartesianControl.h:98
+
constexpr int VOCAB_CC_CONFIG_GAIN
Controller gain.
Definition: ICartesianControl.h:130
+
constexpr int VOCAB_CC_CONFIG_STREAMING_CMD
Preset streaming command.
Definition: ICartesianControl.h:135
+
constexpr int VOCAB_CC_MOVL_CONTROLLING
Controlling MOVL commands.
Definition: ICartesianControl.h:97
+
constexpr int VOCAB_CC_STOP
Stop control.
Definition: ICartesianControl.h:60
+
constexpr int VOCAB_CC_ACTUATOR_OPEN_GRIPPER
Open gripper.
Definition: ICartesianControl.h:116
+
constexpr int VOCAB_CC_POSE
Achieve pose.
Definition: ICartesianControl.h:76
+
constexpr int VOCAB_CC_MOVL
Linear move to target position.
Definition: ICartesianControl.h:56
+
constexpr int VOCAB_CC_MOVV
Linear move with given velocity.
Definition: ICartesianControl.h:57
+
constexpr int VOCAB_CC_MOVJ_CONTROLLING
Controlling MOVJ commands.
Definition: ICartesianControl.h:96
+
Contains roboticslab::ICartesianSolver.
+
Abstract base class for a cartesian controller.
Definition: ICartesianControl.h:146
+
virtual ~ICartesianControl()=default
Destructor.
+
virtual bool stat(std::vector< double > &x, int *state=nullptr, double *timestamp=nullptr)=0
Current state and position.
+
virtual bool act(int command)=0
Actuate tool.
+
virtual bool tool(const std::vector< double > &x)=0
Change tool.
+
virtual bool relj(const std::vector< double > &xd)=0
Move in joint space, relative coordinates.
+
virtual bool setParameter(int vocab, double value)=0
Set a configuration parameter.
+
virtual bool setParameters(const std::map< int, double > ¶ms)=0
Set multiple configuration parameters.
+
virtual bool getParameter(int vocab, double *value)=0
Retrieve a configuration parameter.
+
virtual bool movl(const std::vector< double > &xd)=0
Linear move to target position.
+
virtual bool wait(double timeout=0.0)=0
Wait until completion.
+
virtual void twist(const std::vector< double > &xdot)=0
Instantaneous velocity steps.
+
virtual bool gcmp()=0
Gravity compensation.
+
virtual bool movv(const std::vector< double > &xdotd)=0
Linear move with given velocity.
+
virtual bool inv(const std::vector< double > &xd, std::vector< double > &q)=0
Inverse kinematics.
+
virtual bool getParameters(std::map< int, double > ¶ms)=0
Retrieve multiple configuration parameters.
+
virtual void pose(const std::vector< double > &x)=0
Achieve pose.
+
virtual bool forc(const std::vector< double > &fd)=0
Force control.
+
virtual bool movj(const std::vector< double > &xd)=0
Move in joint space, absolute coordinates.
+
virtual bool stopControl()=0
Stop control.
+
virtual void wrench(const std::vector< double > &w)=0
Exert force.
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/ICartesianSolver_8h.html b/ICartesianSolver_8h.html
new file mode 100644
index 000000000..16464263a
--- /dev/null
+++ b/ICartesianSolver_8h.html
@@ -0,0 +1,108 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/ICartesianSolver.h File Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Contains roboticslab::ICartesianSolver .
+More...
+
#include <vector>
+#include <yarp/os/Vocab.h>
+
+
Go to the source code of this file.
+
+
+ roboticslab
+ The main, catch-all namespace for Robotics Lab UC3M.
+
+
+
+
+
+
+
diff --git a/ICartesianSolver_8h_source.html b/ICartesianSolver_8h_source.html
new file mode 100644
index 000000000..d279db559
--- /dev/null
+++ b/ICartesianSolver_8h_source.html
@@ -0,0 +1,165 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/ICartesianSolver.h Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Go to the documentation of this file.
+
+
3 #ifndef __I_CARTESIAN_SOLVER__
+
4 #define __I_CARTESIAN_SOLVER__
+
+
+
7 #include <yarp/os/Vocab.h>
+
+
+
+
+
+
+
+
+
+
+
29 TCP_FRAME = yarp::os::createVocab32(
'c' ,
'p' ,
'f' ,
't' )
+
+
+
+
+
+
+
+
+
58 virtual bool appendLink (
const std::vector<double>& x) = 0;
+
+
+
+
+
83 const std::vector<double> &x_new_old,
+
84 std::vector<double> &x_new_obj) = 0;
+
+
96 virtual bool fwdKin (
const std::vector<double> &q, std::vector<double> &x) = 0;
+
+
116 virtual bool poseDiff (
const std::vector<double> &xLhs,
const std::vector<double> &xRhs, std::vector<double> &xOut) = 0;
+
+
130 virtual bool invKin (
const std::vector<double> &xd,
const std::vector<double> &qGuess, std::vector<double> &q,
+
+
+
145 virtual bool diffInvKin (
const std::vector<double> &q,
const std::vector<double> &xdot, std::vector<double> &qdot,
+
+
+
160 virtual bool invDyn (
const std::vector<double> &q, std::vector<double> &t) = 0;
+
+
162 #ifndef SWIG_PREPROCESSOR_SHOULD_SKIP_THIS
+
178 [[deprecated(
"use `const std::vector<double> &ftip` signature instead" )]]
+
179 virtual bool invDyn (
const std::vector<double> &q,
const std::vector<double> &qdot,
const std::vector<double> &qdotdot,
+
180 const std::vector<std::vector<double>> &fexts, std::vector<double> &t)
+
+
182 return invDyn (q, qdot, qdotdot, fexts.back(), t);
+
+
+
+
202 virtual bool invDyn (
const std::vector<double> &q,
const std::vector<double> &qdot,
const std::vector<double> &qdotdot,
+
+
+
+
+
+
+
Abstract base class for a cartesian solver.
Definition: ICartesianSolver.h:23
+
virtual int getNumTcps()=0
Get number of TCPs for which the solver has been configured.
+
virtual bool appendLink(const std::vector< double > &x)=0
Append an additional link.
+
virtual bool invDyn(const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< double > &ftip, std::vector< double > &t, reference_frame frame=BASE_FRAME)=0
Perform inverse dynamics.
+
virtual bool fwdKin(const std::vector< double > &q, std::vector< double > &x)=0
Perform forward kinematics.
+
virtual bool restoreOriginalChain()=0
Restore original kinematic chain.
+
virtual bool invKin(const std::vector< double > &xd, const std::vector< double > &qGuess, std::vector< double > &q, reference_frame frame=BASE_FRAME)=0
Perform inverse kinematics.
+
virtual bool changeOrigin(const std::vector< double > &x_old_obj, const std::vector< double > &x_new_old, std::vector< double > &x_new_obj)=0
Change origin in which a pose is expressed.
+
virtual ~ICartesianSolver()=default
Destructor.
+
virtual bool diffInvKin(const std::vector< double > &q, const std::vector< double > &xdot, std::vector< double > &qdot, reference_frame frame=BASE_FRAME)=0
Perform differential inverse kinematics.
+
virtual bool invDyn(const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< std::vector< double >> &fexts, std::vector< double > &t)
Perform inverse dynamics.
Definition: ICartesianSolver.h:179
+
virtual int getNumJoints()=0
Get number of joints for which the solver has been configured.
+
reference_frame
Lists supported reference frames.
Definition: ICartesianSolver.h:27
+
@ TCP_FRAME
End-effector frame (TCP)
Definition: ICartesianSolver.h:29
+
@ BASE_FRAME
Base frame.
Definition: ICartesianSolver.h:28
+
virtual bool poseDiff(const std::vector< double > &xLhs, const std::vector< double > &xRhs, std::vector< double > &xOut)=0
Obtain difference between supplied pose inputs.
+
virtual bool invDyn(const std::vector< double > &q, std::vector< double > &t)=0
Perform inverse dynamics.
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/KdlSolver_8hpp_source.html b/KdlSolver_8hpp_source.html
new file mode 100644
index 000000000..ba55360ea
--- /dev/null
+++ b/KdlSolver_8hpp_source.html
@@ -0,0 +1,192 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/KdlSolver/KdlSolver.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __KDL_SOLVER_HPP__
+
4 #define __KDL_SOLVER_HPP__
+
+
+
+
8 #include <yarp/dev/DeviceDriver.h>
+
+
10 #include <kdl/chain.hpp>
+
11 #include <kdl/chainfksolver.hpp>
+
12 #include <kdl/chainiksolver.hpp>
+
13 #include <kdl/chainidsolver.hpp>
+
+
+
16 #include "LogComponent.hpp"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
45 bool appendLink (
const std::vector<double>& x)
override ;
+
+
+
+
+
+
+
52 const std::vector<double> &x_new_old,
+
53 std::vector<double> &x_new_obj)
override ;
+
+
+
56 bool fwdKin (
const std::vector<double> &q, std::vector<double> &x)
override ;
+
+
+
59 bool poseDiff (
const std::vector<double> &xLhs,
const std::vector<double> &xRhs, std::vector<double> &xOut)
override ;
+
+
+
62 bool invKin (
const std::vector<double> &xd,
const std::vector<double> &qGuess, std::vector<double> &q,
+
+
+
+
66 bool diffInvKin (
const std::vector<double> &q,
const std::vector<double> &xdot, std::vector<double> &qdot,
+
+
+
+
70 bool invDyn (
const std::vector<double> &q, std::vector<double> &t)
override ;
+
+
+
73 bool invDyn (
const std::vector<double> &q,
const std::vector<double> &qdot,
const std::vector<double> &qdotdot,
+
74 const std::vector<double> &ftip, std::vector<double> &t,
reference_frame frame)
override ;
+
+
+
+
78 bool open(yarp::os::Searchable & config)
override ;
+
79 bool close()
override ;
+
+
+
82 inline const yarp::os::LogComponent & logc()
const
+
83 {
return !isQuiet ? KDLS() : KDLS_QUIET(); }
+
+
85 mutable std::mutex mtx;
+
+
+
+
+
+
93 KDL::ChainFkSolverPos * fkSolverPos {
nullptr };
+
94 KDL::ChainIkSolverPos * ikSolverPos {
nullptr };
+
95 KDL::ChainIkSolverVel * ikSolverVel {
nullptr };
+
96 KDL::ChainIdSolver * idSolver {
nullptr };
+
+
+
+
+
+
+
+
Contains roboticslab::ICartesianSolver.
+
Abstract base class for a cartesian solver.
Definition: ICartesianSolver.h:23
+
reference_frame
Lists supported reference frames.
Definition: ICartesianSolver.h:27
+
The KdlSolver class implements ICartesianSolver.
Definition: KdlSolver.hpp:34
+
bool restoreOriginalChain() override
Restore original kinematic chain.
Definition: ICartesianSolverImpl.cpp:51
+
bool invDyn(const std::vector< double > &q, std::vector< double > &t) override
Perform inverse dynamics.
Definition: ICartesianSolverImpl.cpp:225
+
bool poseDiff(const std::vector< double > &xLhs, const std::vector< double > &xRhs, std::vector< double > &xOut) override
Obtain difference between supplied pose inputs.
Definition: ICartesianSolverImpl.cpp:103
+
bool invKin(const std::vector< double > &xd, const std::vector< double > &qGuess, std::vector< double > &q, reference_frame frame) override
Perform inverse kinematics.
Definition: ICartesianSolverImpl.cpp:116
+
bool changeOrigin(const std::vector< double > &x_old_obj, const std::vector< double > &x_new_old, std::vector< double > &x_new_obj) override
Change origin in which a pose is expressed.
Definition: ICartesianSolverImpl.cpp:67
+
KDL::Chain chain
Definition: KdlSolver.hpp:88
+
int getNumJoints() override
Get number of joints for which the solver has been configured.
Definition: ICartesianSolverImpl.cpp:19
+
int getNumTcps() override
Get number of TCPs for which the solver has been configured.
Definition: ICartesianSolverImpl.cpp:26
+
bool diffInvKin(const std::vector< double > &q, const std::vector< double > &xdot, std::vector< double > &qdot, reference_frame frame) override
Perform differential inverse kinematics.
Definition: ICartesianSolverImpl.cpp:169
+
bool fwdKin(const std::vector< double > &q, std::vector< double > &x) override
Perform forward kinematics.
Definition: ICartesianSolverImpl.cpp:80
+
KDL::Chain originalChain
Definition: KdlSolver.hpp:91
+
bool appendLink(const std::vector< double > &x) override
Append an additional link.
Definition: ICartesianSolverImpl.cpp:33
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/KdlTreeSolver_8hpp_source.html b/KdlTreeSolver_8hpp_source.html
new file mode 100644
index 000000000..024269ef4
--- /dev/null
+++ b/KdlTreeSolver_8hpp_source.html
@@ -0,0 +1,190 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/KdlTreeSolver/KdlTreeSolver.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __KDL_TREE_SOLVER_HPP__
+
4 #define __KDL_TREE_SOLVER_HPP__
+
+
+
+
+
+
10 #include <yarp/dev/DeviceDriver.h>
+
+
12 #include <kdl/tree.hpp>
+
13 #include <kdl/treefksolver.hpp>
+
14 #include <kdl/treeiksolver.hpp>
+
15 #include <kdl/treeidsolver.hpp>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
52 bool appendLink (
const std::vector<double> & x)
override ;
+
+
+
+
+
+
+
59 const std::vector<double> & x_new_old,
+
60 std::vector<double> & x_new_obj)
override ;
+
+
+
63 bool fwdKin (
const std::vector<double> & q, std::vector<double> & x)
override ;
+
+
+
66 bool poseDiff (
const std::vector<double> & xLhs,
const std::vector<double> & xRhs, std::vector<double> & xOut)
override ;
+
+
+
69 bool invKin (
const std::vector<double> & xd,
const std::vector<double> & qGuess, std::vector<double> & q,
+
+
+
+
73 bool diffInvKin (
const std::vector<double> & q,
const std::vector<double> & xdot, std::vector<double> & qdot,
+
+
+
+
77 bool invDyn (
const std::vector<double> & q, std::vector<double> & t)
override ;
+
+
+
80 bool invDyn (
const std::vector<double> & q,
const std::vector<double> & qdot,
const std::vector<double> & qdotdot,
+
81 const std::vector<double> & ftip, std::vector<double> & t,
reference_frame frame)
override ;
+
+
+
+
85 bool open(yarp::os::Searchable & config)
override ;
+
+
87 bool close()
override ;
+
+
+
90 std::vector<std::string> endpoints;
+
91 std::map<std::string, std::string> mergedEndpoints;
+
+
93 KDL::TreeFkSolverPos * fkSolverPos;
+
94 KDL::TreeIkSolverPos * ikSolverPos;
+
95 KDL::TreeIkSolverVel * ikSolverVel;
+
96 KDL::TreeIdSolver * idSolver;
+
+
+
+
+
+
Contains roboticslab::ICartesianSolver.
+
Abstract base class for a cartesian solver.
Definition: ICartesianSolver.h:23
+
reference_frame
Lists supported reference frames.
Definition: ICartesianSolver.h:27
+
The KdlTreeSolver class implements ICartesianSolver.
Definition: KdlTreeSolver.hpp:35
+
bool poseDiff(const std::vector< double > &xLhs, const std::vector< double > &xRhs, std::vector< double > &xOut) override
Obtain difference between supplied pose inputs.
Definition: ICartesianSolverImpl.cpp:103
+
bool invDyn(const std::vector< double > &q, std::vector< double > &t) override
Perform inverse dynamics.
Definition: ICartesianSolverImpl.cpp:262
+
bool diffInvKin(const std::vector< double > &q, const std::vector< double > &xdot, std::vector< double > &qdot, reference_frame frame) override
Perform differential inverse kinematics.
Definition: ICartesianSolverImpl.cpp:193
+
bool restoreOriginalChain() override
Restore original kinematic chain.
Definition: ICartesianSolverImpl.cpp:42
+
bool invKin(const std::vector< double > &xd, const std::vector< double > &qGuess, std::vector< double > &q, reference_frame frame) override
Perform inverse kinematics.
Definition: ICartesianSolverImpl.cpp:127
+
int getNumJoints() override
Get number of joints for which the solver has been configured.
Definition: ICartesianSolverImpl.cpp:20
+
bool fwdKin(const std::vector< double > &q, std::vector< double > &x) override
Perform forward kinematics.
Definition: ICartesianSolverImpl.cpp:73
+
bool appendLink(const std::vector< double > &x) override
Append an additional link.
Definition: ICartesianSolverImpl.cpp:34
+
int getNumTcps() override
Get number of TCPs for which the solver has been configured.
Definition: ICartesianSolverImpl.cpp:27
+
bool changeOrigin(const std::vector< double > &x_old_obj, const std::vector< double > &x_new_old, std::vector< double > &x_new_obj) override
Change origin in which a pose is expressed.
Definition: ICartesianSolverImpl.cpp:50
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/KdlVectorConverter_8hpp_source.html b/KdlVectorConverter_8hpp_source.html
new file mode 100644
index 000000000..ffb27f826
--- /dev/null
+++ b/KdlVectorConverter_8hpp_source.html
@@ -0,0 +1,119 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/KdlVectorConverterLib/KdlVectorConverter.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __KDL_VECTOR_CONVERTER_HPP__
+
4 #define __KDL_VECTOR_CONVERTER_HPP__
+
+
+
+
8 #include <kdl/frames.hpp>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Collection of utilities related to KDL and std::vector classes.
Definition: KdlVectorConverter.cpp:14
+
KDL::Frame vectorToFrame(const std::vector< double > &x)
Convert from std::vector<double> to KDL::Frame.
Definition: KdlVectorConverter.cpp:18
+
std::vector< double > frameToVector(const KDL::Frame &f)
Convert from KDL::Frame to std::vector<double>
Definition: KdlVectorConverter.cpp:41
+
KDL::Twist vectorToTwist(const std::vector< double > &xdot)
Convert from std::vector<double> to KDL::Twist.
Definition: KdlVectorConverter.cpp:60
+
KDL::Wrench vectorToWrench(const std::vector< double > &f)
Convert from std::vector<double> to KDL::Wrench.
Definition: KdlVectorConverter.cpp:100
+
std::vector< double > twistToVector(const KDL::Twist &t)
Convert from KDL::Twist to std::vector<double>
Definition: KdlVectorConverter.cpp:83
+
std::vector< double > wrenchToVector(const KDL::Wrench &w)
Convert from KDL::Wrench to std::vector<double>
Definition: KdlVectorConverter.cpp:123
+
+
+
+
+
diff --git a/KeyboardController_8hpp_source.html b/KeyboardController_8hpp_source.html
new file mode 100644
index 000000000..d6d7f77f5
--- /dev/null
+++ b/KeyboardController_8hpp_source.html
@@ -0,0 +1,188 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/keyboardController/KeyboardController.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __KEYBOARD_CONTROLLER_HPP__
+
4 #define __KEYBOARD_CONTROLLER_HPP__
+
+
+
+
+
+
10 #include <yarp/os/RFModule.h>
+
11 #include <yarp/os/ResourceFinder.h>
+
+
13 #include <yarp/dev/PolyDriver.h>
+
14 #include <yarp/dev/IControlLimits.h>
+
15 #include <yarp/dev/IControlMode.h>
+
16 #include <yarp/dev/IEncoders.h>
+
17 #include <yarp/dev/IVelocityControl.h>
+
+
19 #include "LinearTrajectoryThread.hpp"
+
+
21 #include "KinematicRepresentation.hpp"
+
+
+
+
+
+
+
+
+
+
+
+
42 enum joint { Q1 = 0, Q2, Q3, Q4, Q5, Q6, Q7, Q8, Q9, MAX_JOINTS };
+
43 enum cart { X = 0, Y, Z, ROTX, ROTY, ROTZ, NUM_CART_COORDS };
+
+
45 bool configure(yarp::os::ResourceFinder & rf)
override ;
+
46 bool updateModule()
override ;
+
47 bool interruptModule()
override ;
+
48 double getPeriod()
override ;
+
49 bool close()
override ;
+
+
+
52 enum control_modes { NOT_CONTROLLING, JOINT_MODE, CARTESIAN_MODE };
+
+
54 static const std::plus<double> increment_functor;
+
55 static const std::minus<double> decrement_functor;
+
+
57 template <
typename func>
+
58 void incrementOrDecrementJointVelocity(joint q, func op);
+
+
60 template <
typename func>
+
61 void incrementOrDecrementCartesianVelocity(cart coord, func op);
+
+
63 void toggleReferenceFrame();
+
+
65 void actuateTool(
int command);
+
+
67 void printJointPositions();
+
68 void printCartesianPositions();
+
+
+
+
+
+
+
+
+
+
78 std::string angleRepr;
+
+
80 control_modes controlMode {NOT_CONTROLLING};
+
+
82 bool usingThread {
false };
+
+
+
85 yarp::dev::PolyDriver controlBoardDevice;
+
86 yarp::dev::PolyDriver cartesianControlDevice;
+
+
88 yarp::dev::IEncoders * iEncoders {
nullptr };
+
89 yarp::dev::IControlMode * iControlMode {
nullptr };
+
90 yarp::dev::IControlLimits * iControlLimits {
nullptr };
+
91 yarp::dev::IVelocityControl * iVelocityControl {
nullptr };
+
+
+
+
95 std::vector<double> maxVelocityLimits;
+
96 std::vector<double> currentJointVels;
+
97 std::vector<double> currentCartVels;
+
+
+
+
+
+
Contains roboticslab::ICartesianControl and related vocabs.
+
constexpr int VOCAB_CC_ACTUATOR_NONE
No actuator or no action.
Definition: ICartesianControl.h:114
+
Abstract base class for a cartesian controller.
Definition: ICartesianControl.h:146
+
reference_frame
Lists supported reference frames.
Definition: ICartesianSolver.h:27
+
@ BASE_FRAME
Base frame.
Definition: ICartesianSolver.h:28
+
Sends streaming commands to the cartesian controller from a standard keyboard.
Definition: KeyboardController.hpp:36
+
Periodic thread that encapsulates a linear trajectory.
Definition: LinearTrajectoryThread.hpp:24
+
orientation_system
Lists available rotational representations.
Definition: KinematicRepresentation.hpp:36
+
@ AXIS_ANGLE
(axis_x, axis_y, axis_z, rotation angle) [axis needs not to be normalized]
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/KinematicRepresentation_8hpp_source.html b/KinematicRepresentation_8hpp_source.html
new file mode 100644
index 000000000..4bdf7a109
--- /dev/null
+++ b/KinematicRepresentation_8hpp_source.html
@@ -0,0 +1,185 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/KinematicRepresentationLib/KinematicRepresentation.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __KINEMATIC_REPRESENTATION_HPP__
+
4 #define __KINEMATIC_REPRESENTATION_HPP__
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
68 bool encodePose (
const std::vector<double> & x_in, std::vector<double> & x_out,
+
+
+
85 bool decodePose (
const std::vector<double> & x_in, std::vector<double> & x_out,
+
+
+
104 bool encodeVelocity (
const std::vector<double> & x_in,
const std::vector<double> & xdot_in,
+
+
+
+
124 bool decodeVelocity (
const std::vector<double> & x_in,
const std::vector<double> & xdot_in,
+
+
+
+
145 bool encodeAcceleration (
const std::vector<double> & x_in,
const std::vector<double> & xdot_in,
+
146 const std::vector<double> & xdotdot_in, std::vector<double> & xdotdot_out,
+
+
+
167 bool decodeAcceleration (
const std::vector<double> & x_in,
const std::vector<double> & xdot_in,
+
168 const std::vector<double> & xdotdot_in, std::vector<double> & xdotdot_out,
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Collection of static methods to perform geometric transformations.
Definition: KinematicRepresentation.cpp:72
+
coordinate_system
Lists available translational representations.
Definition: KinematicRepresentation.hpp:27
+
@ SPHERICAL
(radial distance, polar angle, azimuthal angle)
+
@ NONE
omit coordinate system in resulting combined coord+orient representation
+
@ CARTESIAN
(x distance, y distance, z distance)
+
@ CYLINDRICAL
(radial distance, azimuthal angle, height)
+
double radToDeg(double rad)
Converts radians to degrees.
Definition: KinematicRepresentation.cpp:523
+
bool encodePose(const std::vector< double > &x_in, std::vector< double > &x_out, coordinate_system coord, orientation_system orient, angular_units angle)
Converts the translation and rotation values of a specific pose to coordinate_system::CARTESIAN and o...
Definition: KinematicRepresentation.cpp:76
+
bool parseEnumerator(const std::string &str, coordinate_system *coord, coordinate_system fallback)
Parses input string, returns matching enumerator value.
Definition: KinematicRepresentation.cpp:530
+
double degToRad(double deg)
Converts degrees to radians.
Definition: KinematicRepresentation.cpp:516
+
bool encodeAcceleration(const std::vector< double > &x_in, const std::vector< double > &xdot_in, const std::vector< double > &xdotdot_in, std::vector< double > &xdotdot_out, coordinate_system coord, orientation_system orient, angular_units angle)
Converts the translation and rotation values of a specific acceleration to coordinate_system::CARTESI...
Definition: KinematicRepresentation.cpp:496
+
orientation_system
Lists available rotational representations.
Definition: KinematicRepresentation.hpp:36
+
@ RPY
fixed axes, roll is axis_x
+
@ EULER_YZ
as EULER_ZYZ, preceded by rotation about the azimuthal angle got from x-y coordinates
+
@ AXIS_ANGLE
(axis_x, axis_y, axis_z, rotation angle) [axis needs not to be normalized]
+
+
@ NONE
omit orientation system in resulting combined coord+orient representation
+
@ POLAR_AZIMUTH
(polar angle, azimuthal angle)
+
@ AXIS_ANGLE_SCALED
(axis_x, axis_y, axis_z) [axis' norm is the rotation angle]
+
bool decodeVelocity(const std::vector< double > &x_in, const std::vector< double > &xdot_in, std::vector< double > &xdot_out, coordinate_system coord, orientation_system orient, angular_units angle)
Converts the translation and rotation values of a specific velocity to the chosen representation syst...
Definition: KinematicRepresentation.cpp:403
+
angular_units
Lists recognized angular units.
Definition: KinematicRepresentation.hpp:48
+
+
+
bool encodeVelocity(const std::vector< double > &x_in, const std::vector< double > &xdot_in, std::vector< double > &xdot_out, coordinate_system coord, orientation_system orient, angular_units angle)
Converts the translation and rotation values of a specific velocity to coordinate_system::CARTESIAN a...
Definition: KinematicRepresentation.cpp:307
+
bool decodePose(const std::vector< double > &x_in, std::vector< double > &x_out, coordinate_system coord, orientation_system orient, angular_units angle)
Converts the translation and rotation values of a specific pose to the chosen representation systems.
Definition: KinematicRepresentation.cpp:187
+
bool decodeAcceleration(const std::vector< double > &x_in, const std::vector< double > &xdot_in, const std::vector< double > &xdotdot_in, std::vector< double > &xdotdot_out, coordinate_system coord, orientation_system orient, angular_units angle)
Converts the translation and rotation values of a specific acceleration to the chosen representation ...
Definition: KinematicRepresentation.cpp:506
+
+
+
+
+
diff --git a/LeapMotionSensorDevice_8hpp_source.html b/LeapMotionSensorDevice_8hpp_source.html
new file mode 100644
index 000000000..6dcc6aa73
--- /dev/null
+++ b/LeapMotionSensorDevice_8hpp_source.html
@@ -0,0 +1,150 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/streamingDeviceController/LeapMotionSensorDevice.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
1 #ifndef __LEAP_MOTION_SENSOR_DEVICE_HPP__
+
2 #define __LEAP_MOTION_SENSOR_DEVICE_HPP__
+
+
4 #include "StreamingDevice.hpp"
+
+
+
+
8 #include <kdl/frames.hpp>
+
+
10 #include <yarp/dev/IAnalogSensor.h>
+
+
+
+
+
+
+
+
+
+
+
+
29 bool initialize (
bool usingStreamingPreset)
override ;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
43 yarp::dev::IAnalogSensor * iAnalogSensor;
+
+
+
+
47 std::vector<double> initialTcpOffset;
+
48 std::vector<double> initialLeapOffset;
+
+
50 KDL::Frame frame_base_leap, frame_ee_leap, frame_leap_ee;
+
+
52 KDL::Frame previousPose;
+
53 double previousTimestamp;
+
+
+
+
+
+
+
+
+
Represents a LeapMotion device wrapped as an analog sensor by YARP.
Definition: LeapMotionSensorDevice.hpp:22
+
void sendMovementCommand(double timestamp) override
Sends movement command to the cartesian controller.
Definition: LeapMotionSensorDevice.cpp:212
+
LeapMotionSensorDevice(yarp::os::Searchable &config, bool usingPose)
Constructor.
Definition: LeapMotionSensorDevice.cpp:18
+
bool transformData(double scaling) override
Performs required operations on stored data.
Definition: LeapMotionSensorDevice.cpp:141
+
bool initialize(bool usingStreamingPreset) override
Perform any custom initialization needed. This method is called after the successful creation of the ...
Definition: LeapMotionSensorDevice.cpp:59
+
int getActuatorState() override
If actuator command data is available, return its current state.
Definition: LeapMotionSensorDevice.cpp:178
+
bool acquireInterfaces() override
Acquires plugin interfaces.
Definition: LeapMotionSensorDevice.cpp:46
+
void stopMotion() override
Sends a movement command that would stop motion.
Definition: LeapMotionSensorDevice.hpp:39
+
bool acquireData() override
Acquires data from remote device.
Definition: LeapMotionSensorDevice.cpp:107
+
Abstract class for a YARP streaming device.
Definition: StreamingDevice.hpp:46
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/LinearTrajectoryThread_8hpp_source.html b/LinearTrajectoryThread_8hpp_source.html
new file mode 100644
index 000000000..84cc723be
--- /dev/null
+++ b/LinearTrajectoryThread_8hpp_source.html
@@ -0,0 +1,133 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/keyboardController/LinearTrajectoryThread.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __LINEAR_TRAJECTORY_THREAD_HPP__
+
4 #define __LINEAR_TRAJECTORY_THREAD_HPP__
+
+
+
+
+
9 #include <yarp/os/PeriodicThread.h>
+
+
11 #include <kdl/trajectory.hpp>
+
+
+
+
+
+
+
+
+
+
+
+
+
29 bool checkStreamingConfig();
+
30 bool configure(
const std::vector<double> & vels);
+
31 void useTcpFrame(
bool enableTcpFrame) { usingTcpFrame = enableTcpFrame; }
+
+
+
+
+
+
+
+
39 KDL::Trajectory * trajectory {
nullptr };
+
40 double startTime {0.0};
+
41 bool usingStreamingCommandConfig {
false };
+
42 bool usingTcpFrame {
false };
+
43 std::vector<double> deltaX;
+
44 mutable std::mutex mtx;
+
+
+
+
+
+
Contains roboticslab::ICartesianControl and related vocabs.
+
Abstract base class for a cartesian controller.
Definition: ICartesianControl.h:146
+
Periodic thread that encapsulates a linear trajectory.
Definition: LinearTrajectoryThread.hpp:24
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/MatrixExponential_8hpp_source.html b/MatrixExponential_8hpp_source.html
new file mode 100644
index 000000000..e595f5bed
--- /dev/null
+++ b/MatrixExponential_8hpp_source.html
@@ -0,0 +1,142 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/ScrewTheoryLib/MatrixExponential.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __MATRIX_EXPONENTIAL_HPP__
+
4 #define __MATRIX_EXPONENTIAL_HPP__
+
+
6 #include <kdl/frames.hpp>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
36 MatrixExponential (
motion motionType,
const KDL::Vector & axis,
const KDL::Vector & origin = KDL::Vector::Zero());
+
+
45 KDL::Frame
asFrame (
double theta)
const ;
+
+
+
53 {
return motionType; }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Abstraction of a term in a product of exponentials (POE) formula.
Definition: MatrixExponential.hpp:19
+
MatrixExponential cloneWithBase(const KDL::Frame &H_new_old) const
Clones this instance and refers the internal coordinates of the screw to a different base.
Definition: MatrixExponential.cpp:70
+
motion
Lists available screw motion types.
Definition: MatrixExponential.hpp:23
+
@ TRANSLATION
Prismatic joint (infinite-pitch twist).
Definition: MatrixExponential.hpp:25
+
@ ROTATION
Revolute joint (zero-pitch twist).
Definition: MatrixExponential.hpp:24
+
KDL::Frame asFrame(double theta) const
Evaluates this term for the given magnitude of the screw.
Definition: MatrixExponential.cpp:36
+
const KDL::Vector & getOrigin() const
A point along the screw axis.
Definition: MatrixExponential.hpp:68
+
const KDL::Vector & getAxis() const
Screw axis.
Definition: MatrixExponential.hpp:60
+
motion getMotionType() const
Retrieves the motion type of this screw.
Definition: MatrixExponential.hpp:52
+
void changeBase(const KDL::Frame &H_new_old)
Refers the internal coordinates of this screw to a different base.
Definition: MatrixExponential.cpp:58
+
MatrixExponential(motion motionType, const KDL::Vector &axis, const KDL::Vector &origin=KDL::Vector::Zero())
Constructor.
Definition: MatrixExponential.cpp:26
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/ProductOfExponentials_8hpp_source.html b/ProductOfExponentials_8hpp_source.html
new file mode 100644
index 000000000..dea58e9bd
--- /dev/null
+++ b/ProductOfExponentials_8hpp_source.html
@@ -0,0 +1,159 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/ScrewTheoryLib/ProductOfExponentials.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __PRODUCT_OF_EXPONENTIALS_HPP__
+
4 #define __PRODUCT_OF_EXPONENTIALS_HPP__
+
+
+
+
8 #include <kdl/chain.hpp>
+
9 #include <kdl/frames.hpp>
+
10 #include <kdl/jntarray.hpp>
+
+
12 #include "MatrixExponential.hpp"
+
+
+
+
+
+
+
+
35 explicit PoeExpression (
const KDL::Frame & H_S_T = KDL::Frame::Identity()) : H_S_T(H_S_T) {}
+
+
+
+
+
61 void append (
const PoeExpression & poe,
const KDL::Frame & H_new_old = KDL::Frame::Identity());
+
+
+
+
+
+
77 {
return exps.size(); }
+
+
+
87 {
return exps.at(i); }
+
+
+
+
+
105 { H_S_T = H_S_T * H_new_old; }
+
+
116 bool evaluate (
const KDL::JntArray & q, KDL::Frame & H)
const ;
+
+
+
+
+
+
+
+
+
+
+
161 std::vector<MatrixExponential> exps;
+
+
+
+
+
+
+
Abstraction of a term in a product of exponentials (POE) formula.
Definition: MatrixExponential.hpp:19
+
MatrixExponential cloneWithBase(const KDL::Frame &H_new_old) const
Clones this instance and refers the internal coordinates of the screw to a different base.
Definition: MatrixExponential.cpp:70
+
Abstraction of a product of exponentials (POE) formula.
Definition: ProductOfExponentials.hpp:28
+
KDL::Chain toChain() const
Makes a KDL::Chain from this instance.
Definition: ProductOfExponentials.cpp:138
+
void reverseSelf()
Inverts this POE formula.
Definition: ProductOfExponentials.cpp:104
+
bool evaluate(const KDL::JntArray &q, KDL::Frame &H) const
Performs forward kinematics.
Definition: ProductOfExponentials.cpp:82
+
static PoeExpression fromChain(const KDL::Chain &chain)
Builds a PoeExpression from a KDL::Chain.
Definition: ProductOfExponentials.cpp:167
+
void changeBaseFrame(const KDL::Frame &H_new_old)
Refers the internal coordinates of this POE to a different base.
Definition: ProductOfExponentials.cpp:70
+
const MatrixExponential & exponentialAtJoint(int i) const
Retrieves a term of the POE formula.
Definition: ProductOfExponentials.hpp:86
+
const KDL::Frame & getTransform() const
Retrieves the transformation between base and tool frames.
Definition: ProductOfExponentials.hpp:68
+
void append(const MatrixExponential &exp, const KDL::Frame &H_new_old=KDL::Frame::Identity())
Appends a new term to this POE formula.
Definition: ProductOfExponentials.hpp:47
+
int size() const
Size of this POE.
Definition: ProductOfExponentials.hpp:76
+
PoeExpression(const KDL::Frame &H_S_T=KDL::Frame::Identity())
Constructor.
Definition: ProductOfExponentials.hpp:35
+
PoeExpression makeReverse() const
Creates a new POE entity from the inverse of this POE formula.
Definition: ProductOfExponentials.cpp:118
+
void changeToolFrame(const KDL::Frame &H_new_old)
Updates the tool frame.
Definition: ProductOfExponentials.hpp:104
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/ScrewTheoryIkProblem_8hpp_source.html b/ScrewTheoryIkProblem_8hpp_source.html
new file mode 100644
index 000000000..d06eae32d
--- /dev/null
+++ b/ScrewTheoryIkProblem_8hpp_source.html
@@ -0,0 +1,237 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/ScrewTheoryLib/ScrewTheoryIkProblem.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __SCREW_THEORY_IK_PROBLEM_HPP__
+
4 #define __SCREW_THEORY_IK_PROBLEM_HPP__
+
+
+
+
+
9 #include <kdl/frames.hpp>
+
10 #include <kdl/jntarray.hpp>
+
+
12 #include "ProductOfExponentials.hpp"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
98 using Steps = std::vector<const ScrewTheoryIkSubproblem *>;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
151 using Frames = std::vector<KDL::Frame>;
+
152 using PoeTerms = std::vector<poe_term>;
+
+
+
155 ScrewTheoryIkProblem(
const PoeExpression & poe,
const Steps & steps,
bool reversed);
+
+
157 void recalculateFrames(
const Solutions &
solutions , Frames & frames, PoeTerms & poeTerms);
+
158 bool recalculateFrames(
const Solutions &
solutions , Frames & frames, PoeTerms & poeTerms,
bool backwards);
+
+
160 KDL::Frame transformPoint(
const KDL::JntArray & jointValues,
const PoeTerms & poeTerms);
+
+
162 const PoeExpression poe;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
188 PoeTerm () : known(
false ), simplified(
false ) {}
+
189 bool known, simplified;
+
+
+
+
+
+
+
+
207 static std::vector<KDL::Vector> searchPoints(
const PoeExpression & poe);
+
+
+
+
211 void refreshSimplificationState();
+
+
213 void simplify(
int depth);
+
214 void simplifyWithPadenKahanOne(
const KDL::Vector & point);
+
215 void simplifyWithPadenKahanThree(
const KDL::Vector & point);
+
216 void simplifyWithPardosOne();
+
+
+
+
+
+
222 std::vector<KDL::Vector> points;
+
223 std::vector<KDL::Vector> testPoints;
+
+
225 std::vector<PoeTerm> poeTerms;
+
+
227 static const int MAX_SIMPLIFICATION_DEPTH = 2;
+
+
+
+
+
+
Abstraction of a product of exponentials (POE) formula.
Definition: ProductOfExponentials.hpp:28
+
Automated IK solution finder.
Definition: ScrewTheoryIkProblem.hpp:183
+
ScrewTheoryIkProblem * build()
Finds a valid sequence of geometric subproblems that solve a global IK problem.
Definition: ScrewTheoryIkProblemBuilder.cpp:231
+
ScrewTheoryIkProblemBuilder(const PoeExpression &poe)
Constructor.
Definition: ScrewTheoryIkProblemBuilder.cpp:144
+
Proxy IK problem solver class that iterates over a sequence of subproblems.
Definition: ScrewTheoryIkProblem.hpp:95
+
static ScrewTheoryIkProblem * create(const PoeExpression &poe, const Steps &steps, bool reversed=false)
Creates an IK solver instance given a sequence of known subproblems.
Definition: ScrewTheoryIkProblem.cpp:265
+
const Steps & getSteps() const
Solution of the IK problem (if available)
Definition: ScrewTheoryIkProblem.hpp:125
+
bool solve(const KDL::Frame &H_S_T, Solutions &solutions)
Find all available solutions.
Definition: ScrewTheoryIkProblem.cpp:62
+
std::vector< KDL::JntArray > Solutions
Collection of global IK solutions.
Definition: ScrewTheoryIkProblem.hpp:101
+
~ScrewTheoryIkProblem()
Destructor.
Definition: ScrewTheoryIkProblem.cpp:52
+
bool isReversed() const
Whether the computed solution is reversed.
Definition: ScrewTheoryIkProblem.hpp:129
+
std::vector< const ScrewTheoryIkSubproblem * > Steps
Ordered sequence of IK subproblems needed to solve a IK problem.
Definition: ScrewTheoryIkProblem.hpp:98
+
int solutions() const
Number of global IK solutions.
Definition: ScrewTheoryIkProblem.hpp:121
+
Interface shared by all IK subproblems found in Screw Theory applied to Robotics.
Definition: ScrewTheoryIkProblem.hpp:26
+
std::vector< JointIdsToSolutions > Solutions
Collection of local IK solutions.
Definition: ScrewTheoryIkProblem.hpp:35
+
std::vector< JointIdToSolution > JointIdsToSolutions
At least one joint-id+value pair per solution.
Definition: ScrewTheoryIkProblem.hpp:32
+
virtual int solutions() const =0
Number of local IK solutions.
+
virtual ~ScrewTheoryIkSubproblem()=default
Destructor.
+
virtual bool solve(const KDL::Frame &rhs, const KDL::Frame &pointTransform, Solutions &solutions) const =0
Finds a closed geometric solution for this IK subproblem.
+
virtual const char * describe() const =0
Return a human-readable description of this IK subproblem.
+
std::pair< int, double > JointIdToSolution
Maps a joint id to a screw magnitude.
Definition: ScrewTheoryIkProblem.hpp:29
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
Helper structure that holds the state of a POE term.
Definition: ScrewTheoryIkProblem.hpp:187
+
+
+
+
+
diff --git a/ScrewTheoryIkSubproblems_8hpp_source.html b/ScrewTheoryIkSubproblems_8hpp_source.html
new file mode 100644
index 000000000..bec6f2494
--- /dev/null
+++ b/ScrewTheoryIkSubproblems_8hpp_source.html
@@ -0,0 +1,279 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/ScrewTheoryLib/ScrewTheoryIkSubproblems.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __SCREW_THEORY_IK_SUBPROBLEMS_HPP__
+
4 #define __SCREW_THEORY_IK_SUBPROBLEMS_HPP__
+
+
6 #include <kdl/frames.hpp>
+
+
8 #include "ScrewTheoryIkProblem.hpp"
+
9 #include "MatrixExponential.hpp"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
47 const KDL::Rotation axisPow;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
85 const KDL::Vector p, r, axesCross;
+
86 const KDL::Rotation axisPow1, axisPow2;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
123 const KDL::Vector p, k;
+
124 const KDL::Rotation axisPow;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
196 const KDL::Vector p, crossPr2;
+
197 const double crossPr2Norm;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
234 const KDL::Vector p, k;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
272 const KDL::Vector p, n;
+
273 const KDL::Rotation axisPow;
+
+
+
+
+
+
Abstraction of a term in a product of exponentials (POE) formula.
Definition: MatrixExponential.hpp:19
+
First Paden-Kahan subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:24
+
PadenKahanOne(int id, const MatrixExponential &exp, const KDL::Vector &p)
Constructor.
Definition: PadenKahanSubproblems.cpp:13
+
bool solve(const KDL::Frame &rhs, const KDL::Frame &pointTransform, Solutions &solutions) const override
Finds a closed geometric solution for this IK subproblem.
Definition: PadenKahanSubproblems.cpp:22
+
const char * describe() const override
Return a human-readable description of this IK subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:40
+
int solutions() const override
Number of local IK solutions.
Definition: ScrewTheoryIkSubproblems.hpp:37
+
Third Paden-Kahan subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:100
+
bool solve(const KDL::Frame &rhs, const KDL::Frame &pointTransform, Solutions &solutions) const override
Finds a closed geometric solution for this IK subproblem.
Definition: PadenKahanSubproblems.cpp:161
+
const char * describe() const override
Return a human-readable description of this IK subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:117
+
int solutions() const override
Number of local IK solutions.
Definition: ScrewTheoryIkSubproblems.hpp:114
+
PadenKahanThree(int id, const MatrixExponential &exp, const KDL::Vector &p, const KDL::Vector &k)
Constructor.
Definition: PadenKahanSubproblems.cpp:151
+
Second Paden-Kahan subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:60
+
bool solve(const KDL::Frame &rhs, const KDL::Frame &pointTransform, Solutions &solutions) const override
Finds a closed geometric solution for this IK subproblem.
Definition: PadenKahanSubproblems.cpp:64
+
PadenKahanTwo(int id1, int id2, const MatrixExponential &exp1, const MatrixExponential &exp2, const KDL::Vector &p, const KDL::Vector &r)
Constructor.
Definition: PadenKahanSubproblems.cpp:49
+
int solutions() const override
Number of local IK solutions.
Definition: ScrewTheoryIkSubproblems.hpp:76
+
const char * describe() const override
Return a human-readable description of this IK subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:79
+
Fourth Pardos-Gotor subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:248
+
PardosGotorFour(int id1, int id2, const MatrixExponential &exp1, const MatrixExponential &exp2, const KDL::Vector &p)
Constructor.
Definition: PardosGotorSubproblems.cpp:148
+
bool solve(const KDL::Frame &rhs, const KDL::Frame &pointTransform, Solutions &solutions) const override
Finds a closed geometric solution for this IK subproblem.
Definition: PardosGotorSubproblems.cpp:160
+
int solutions() const override
Number of local IK solutions.
Definition: ScrewTheoryIkSubproblems.hpp:263
+
const char * describe() const override
Return a human-readable description of this IK subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:266
+
First Pardos-Gotor subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:137
+
int solutions() const override
Number of local IK solutions.
Definition: ScrewTheoryIkSubproblems.hpp:150
+
bool solve(const KDL::Frame &rhs, const KDL::Frame &pointTransform, Solutions &solutions) const override
Finds a closed geometric solution for this IK subproblem.
Definition: PardosGotorSubproblems.cpp:32
+
PardosGotorOne(int id, const MatrixExponential &exp, const KDL::Vector &p)
Constructor.
Definition: PardosGotorSubproblems.cpp:24
+
const char * describe() const override
Return a human-readable description of this IK subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:153
+
Third Pardos-Gotor subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:211
+
const char * describe() const override
Return a human-readable description of this IK subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:228
+
int solutions() const override
Number of local IK solutions.
Definition: ScrewTheoryIkSubproblems.hpp:225
+
bool solve(const KDL::Frame &rhs, const KDL::Frame &pointTransform, Solutions &solutions) const override
Finds a closed geometric solution for this IK subproblem.
Definition: PardosGotorSubproblems.cpp:107
+
PardosGotorThree(int id, const MatrixExponential &exp, const KDL::Vector &p, const KDL::Vector &k)
Constructor.
Definition: PardosGotorSubproblems.cpp:98
+
Second Pardos-Gotor subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:172
+
bool solve(const KDL::Frame &rhs, const KDL::Frame &pointTransform, Solutions &solutions) const override
Finds a closed geometric solution for this IK subproblem.
Definition: PardosGotorSubproblems.cpp:63
+
PardosGotorTwo(int id1, int id2, const MatrixExponential &exp1, const MatrixExponential &exp2, const KDL::Vector &p)
Constructor.
Definition: PardosGotorSubproblems.cpp:51
+
const char * describe() const override
Return a human-readable description of this IK subproblem.
Definition: ScrewTheoryIkSubproblems.hpp:190
+
int solutions() const override
Number of local IK solutions.
Definition: ScrewTheoryIkSubproblems.hpp:187
+
Interface shared by all IK subproblems found in Screw Theory applied to Robotics.
Definition: ScrewTheoryIkProblem.hpp:26
+
std::vector< JointIdsToSolutions > Solutions
Collection of local IK solutions.
Definition: ScrewTheoryIkProblem.hpp:35
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/ScrewTheoryTools_8hpp_source.html b/ScrewTheoryTools_8hpp_source.html
new file mode 100644
index 000000000..278459c43
--- /dev/null
+++ b/ScrewTheoryTools_8hpp_source.html
@@ -0,0 +1,131 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/ScrewTheoryLib/ScrewTheoryTools.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __SCREW_THEORY_TOOLS_HPP__
+
4 #define __SCREW_THEORY_TOOLS_HPP__
+
+
+
+
8 #include <kdl/frames.hpp>
+
9 #include <kdl/utilities/utility.h>
+
+
+
+
+
+
+
47 return KDL::Rotation(v.x() * v.x(), v.x() * v.y(), v.x() * v.z(),
+
48 v.x() * v.y(), v.y() * v.y(), v.y() * v.z(),
+
49 v.x() * v.z(), v.y() * v.z(), v.z() * v.z());
+
+
+
+
+
66 if (KDL::Equal(std::abs(angle), KDL::PI))
+
+
+
+
70 else if (angle > KDL::PI)
+
+
72 return angle - 2 * KDL::PI;
+
+
74 else if (angle < -KDL::PI)
+
+
76 return angle + 2 * KDL::PI;
+
+
+
+
+
+
+
+
+
+
+
KDL::Rotation vectorPow2(const KDL::Vector &v)
Multiply a vector by itself to obtain a square matrix.
Definition: ScrewTheoryTools.hpp:45
+
double normalizeAngle(double angle)
Clip an angle value between certain bounds.
Definition: ScrewTheoryTools.hpp:64
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/SpnavSensorDevice_8hpp_source.html b/SpnavSensorDevice_8hpp_source.html
new file mode 100644
index 000000000..967c9ead6
--- /dev/null
+++ b/SpnavSensorDevice_8hpp_source.html
@@ -0,0 +1,142 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/streamingDeviceController/SpnavSensorDevice.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
1 #ifndef __SPNAV_SENSOR_DEVICE_HPP__
+
2 #define __SPNAV_SENSOR_DEVICE_HPP__
+
+
4 #include "StreamingDevice.hpp"
+
+
6 #include <yarp/dev/IAnalogSensor.h>
+
+
+
+
+
+
+
+
21 SpnavSensorDevice (yarp::os::Searchable & config,
bool usingMovi,
double gain = 0.0);
+
+
+
+
25 bool initialize (
bool usingStreamingPreset)
override ;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
40 yarp::dev::IAnalogSensor * iAnalogSensor;
+
+
42 std::vector<double> currentX;
+
+
+
+
+
+
+
+
+
+
+
Represents a spacenav-compatible device, like the SpaceNavigator 6-DOF mouse from 3Dconnexion.
Definition: SpnavSensorDevice.hpp:18
+
bool acquireData() override
Acquires data from remote device.
Definition: SpnavSensorDevice.cpp:66
+
void stopMotion() override
Sends a movement command that would stop motion.
Definition: SpnavSensorDevice.cpp:183
+
void sendMovementCommand(double timestamp) override
Sends movement command to the cartesian controller.
Definition: SpnavSensorDevice.cpp:166
+
bool transformData(double scaling) override
Performs required operations on stored data.
Definition: SpnavSensorDevice.cpp:93
+
SpnavSensorDevice(yarp::os::Searchable &config, bool usingMovi, double gain=0.0)
Constructor.
Definition: SpnavSensorDevice.cpp:10
+
bool acquireInterfaces() override
Acquires plugin interfaces.
Definition: SpnavSensorDevice.cpp:19
+
int getActuatorState() override
If actuator command data is available, return its current state.
Definition: SpnavSensorDevice.cpp:117
+
bool initialize(bool usingStreamingPreset) override
Perform any custom initialization needed. This method is called after the successful creation of the ...
Definition: SpnavSensorDevice.cpp:32
+
bool hasValidMovementData() const override
Checks whether the device may forward acquired and processed data to the controller.
Definition: SpnavSensorDevice.cpp:146
+
Abstract class for a YARP streaming device.
Definition: StreamingDevice.hpp:46
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/StreamingDeviceController_8hpp_source.html b/StreamingDeviceController_8hpp_source.html
new file mode 100644
index 000000000..09ffccfd7
--- /dev/null
+++ b/StreamingDeviceController_8hpp_source.html
@@ -0,0 +1,151 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/streamingDeviceController/StreamingDeviceController.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
1 #ifndef __STREAMING_DEVICE_CONTROLLER_HPP__
+
2 #define __STREAMING_DEVICE_CONTROLLER_HPP__
+
+
+
+
6 #include <yarp/os/Bottle.h>
+
7 #include <yarp/os/BufferedPort.h>
+
8 #include <yarp/os/RFModule.h>
+
9 #include <yarp/os/TypedReaderCallback.h>
+
+
11 #include <yarp/dev/PolyDriver.h>
+
+
13 #include "StreamingDevice.hpp"
+
14 #include "CentroidTransform.hpp"
+
+
+
+
+
+
+
+
30 public yarp::os::TypedReaderCallback<yarp::os::Bottle>
+
+
+
+
+
+
36 bool configure(yarp::os::ResourceFinder & rf)
override ;
+
37 bool updateModule()
override ;
+
38 bool interruptModule()
override ;
+
39 bool close()
override ;
+
40 double getPeriod()
override ;
+
41 void onRead(yarp::os::Bottle & bot)
override ;
+
+
+
44 bool update(
double timestamp);
+
+
+
+
48 yarp::dev::PolyDriver cartesianControlClientDevice;
+
+
+
51 yarp::os::BufferedPort<yarp::os::Bottle> proximityPort;
+
52 int thresholdAlertHigh;
+
53 int thresholdAlertLow;
+
54 bool disableSensorsLowLevel;
+
+
56 yarp::os::BufferedPort<yarp::os::Bottle> centroidPort;
+
+
+
59 yarp::os::BufferedPort<yarp::os::Bottle> syncPort;
+
+
+
+
+
+
+
+
+
+
Contains roboticslab::ICartesianControl and related vocabs.
+
+
Abstract base class for a cartesian controller.
Definition: ICartesianControl.h:146
+
Sends streaming commands to the cartesian controller from a streaming input device like the 3Dconnexi...
Definition: StreamingDeviceController.hpp:31
+
Abstract class for a YARP streaming device.
Definition: StreamingDevice.hpp:46
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/StreamingDevice_8hpp_source.html b/StreamingDevice_8hpp_source.html
new file mode 100644
index 000000000..64f48f665
--- /dev/null
+++ b/StreamingDevice_8hpp_source.html
@@ -0,0 +1,215 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/streamingDeviceController/StreamingDevice.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
1 #ifndef __STREAMING_DEVICE_HPP__
+
2 #define __STREAMING_DEVICE_HPP__
+
+
+
+
+
7 #include <yarp/os/Searchable.h>
+
8 #include <yarp/os/Value.h>
+
9 #include <yarp/dev/PolyDriver.h>
+
+
+
12 #include "CentroidTransform.hpp"
+
+
+
+
+
17 class StreamingDevice;
+
18 class CentroidTransform;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
50 using PolyDriver::isValid;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
100 return actuatorState;
+
+
+
+
+
+
+
+
+
+
+
127 this->iCartesianControl = iCartesianControl;
+
+
+
+
+
+
133 std::vector<double> data;
+
134 std::vector<bool> fixedAxes;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Contains roboticslab::ICartesianControl and related vocabs.
+
+
Abstract base class for a cartesian controller.
Definition: ICartesianControl.h:146
+
Represents an invalid device.
Definition: StreamingDevice.hpp:153
+
void stopMotion() override
Sends a movement command that would stop motion.
Definition: StreamingDevice.hpp:173
+
bool acquireInterfaces() override
Acquires plugin interfaces.
Definition: StreamingDevice.hpp:160
+
bool acquireData() override
Acquires data from remote device.
Definition: StreamingDevice.hpp:165
+
void sendMovementCommand(double timestamp) override
Sends movement command to the cartesian controller.
Definition: StreamingDevice.hpp:170
+
InvalidDevice()
Creates an invalid device.
Definition: StreamingDevice.hpp:156
+
Factory class for creating instances of StreamingDevice.
Definition: StreamingDevice.hpp:26
+
static StreamingDevice * makeDevice(const std::string &deviceName, yarp::os::Searchable &config)
Creates a new YARP device handle.
Definition: StreamingDevice.cpp:15
+
Abstract class for a YARP streaming device.
Definition: StreamingDevice.hpp:46
+
virtual bool transformData(double scaling)
Performs required operations on stored data.
Definition: StreamingDevice.cpp:62
+
virtual bool acquireData()=0
Acquires data from remote device.
+
virtual void stopMotion()=0
Sends a movement command that would stop motion.
+
virtual bool hasValidMovementData() const
Checks whether the device may forward acquired and processed data to the controller.
Definition: StreamingDevice.cpp:79
+
virtual int getActuatorState()
If actuator command data is available, return its current state.
Definition: StreamingDevice.hpp:98
+
virtual void sendMovementCommand(double timestamp)=0
Sends movement command to the cartesian controller.
+
virtual bool acquireInterfaces()=0
Acquires plugin interfaces.
+
void setCartesianControllerHandle(ICartesianControl *iCartesianControl)
Stores handle to an ICartesianControl instance.
Definition: StreamingDevice.hpp:125
+
virtual bool initialize(bool usingStreamingPreset)
Perform any custom initialization needed. This method is called after the successful creation of the ...
Definition: StreamingDevice.hpp:75
+
virtual ~StreamingDevice()
Destructor.
Definition: StreamingDevice.cpp:57
+
void configureFixedAxes(const yarp::os::Value &v)
Stores vector of values representing axes that are always fixed.
Definition: StreamingDevice.cpp:97
+
StreamingDevice(yarp::os::Searchable &config)
Constructor.
Definition: StreamingDevice.cpp:46
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/TrajGen_8hpp_source.html b/TrajGen_8hpp_source.html
new file mode 100644
index 000000000..5c30b4fa2
--- /dev/null
+++ b/TrajGen_8hpp_source.html
@@ -0,0 +1,260 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/AsibotSolver/TrajGen.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __TRAJ_GEN_HPP__
+
4 #define __TRAJ_GEN_HPP__
+
+
6 #include <yarp/os/Log.h>
+
+
+
+
+
+
29 virtual bool configure(
const double xi,
const double xf,
const double _T) = 0;
+
30 virtual bool configure(
const double xi,
const double xf,
const double xdoti,
const double xdotf,
const double _T) = 0;
+
31 virtual double get(
const double ti)
const = 0;
+
32 virtual double getdot(
const double ti)
const = 0;
+
33 virtual double getdotdot(
const double ti)
const = 0;
+
34 virtual bool maxVelBelow(
const double thresVel)
const = 0;
+
35 virtual bool maxAccBelow(
const double thresAcc)
const = 0;
+
36 virtual double getT()
const = 0;
+
37 virtual void dump(
double samples)
const = 0;
+
+
+
+
+
+
+
+
50 bool configure(
const double xi,
const double xf,
const double _T)
+
+
+
+
+
+
+
+
58 bool configure(
const double xi,
const double xf,
const double xdoti,
const double xdotf,
const double _T)
+
+
+
+
+
63 double get(
const double ti)
const
+
+
+
+
+
68 double getdot(
const double ti)
const
+
+
+
+
+
73 double getdotdot(
const double ti)
const
+
+
+
+
+
78 bool maxVelBelow(
const double thresVel)
const
+
+
+
+
+
83 bool maxAccBelow(
const double thresAcc)
const
+
+
+
+
+
+
+
+
+
+
93 void dump(
double samples)
const
+
+
95 for (
double i = 0; i < T; i += (T / samples))
+
+
97 yInfo(
"%05.2f %+02.6f %+02.6f %+02.6f" , i, get(i), getdot(i), getdotdot(i));
+
+
+
+
+
+
+
+
+
+
+
+
+
118 bool configure (
const double xi,
const double xf,
const double _T)
+
+
+
+
+
123 a3 = 2 * (xi - xf) / (T * T * T);
+
124 a2 = (xf - xi) / (T * T) - a3 * T;
+
+
+
+
131 bool configure (
const double xi,
const double xf,
const double xdoti,
const double xdotf,
const double _T)
+
+
+
+
+
136 a3 = 2.0 * (xi - xf) / (T * T * T) + (xdotf + xdoti) / (T * T);
+
137 a2 = (xf - xi - xdoti * T) / (T * T) - a3 * T;
+
+
+
+
144 double get (
const double ti)
const
+
+
+
+
148 return a3 * T * T * T + a2 * T * T + a1 * T + a0;
+
+
150 return a3 * ti * ti * ti + a2 * ti * ti + a1 * ti + a0;
+
+
+
+
+
+
+
160 return 3 * a3 * T * T + 2 * a2 * T + a1;
+
+
162 return 3 * a3 * ti * ti + 2 * a2 * ti + a1;
+
+
+
+
+
+
+
172 return 6 * a3 * T + 2 * a2;
+
+
174 return 6 * a3 * ti + 2 * a2;
+
+
+
+
+
182 return getdot (T / 2) < thresVel;
+
+
+
+
+
+
+
+
+
+
+
+
+
205 void dump (
double samples)
const
+
+
207 for (
double i = 0; i < T; i += (T / samples))
+
+
+
+
+
+
+
214 double a3, a2, a1, a0, T;
+
+
+
+
+
Generates a 1DOF order-one trajectory.
Definition: TrajGen.hpp:46
+
Generates a 1DOF order-three trajectory.
Definition: TrajGen.hpp:111
+
void dump(double samples) const
Definition: TrajGen.hpp:205
+
bool maxVelBelow(const double thresVel) const
Definition: TrajGen.hpp:180
+
bool maxAccBelow(const double thresAcc) const
Definition: TrajGen.hpp:188
+
double getdot(const double ti) const
Definition: TrajGen.hpp:156
+
double getT() const
Definition: TrajGen.hpp:196
+
double getdotdot(const double ti) const
Definition: TrajGen.hpp:168
+
double get(const double ti) const
Definition: TrajGen.hpp:144
+
bool configure(const double xi, const double xf, const double xdoti, const double xdotf, const double _T)
Definition: TrajGen.hpp:131
+
bool configure(const double xi, const double xf, const double _T)
Definition: TrajGen.hpp:118
+
A base class for 1 degree-of-freedom trajectories.
Definition: TrajGen.hpp:26
+
+
+
+
+
diff --git a/TrajectoryThread_8hpp_source.html b/TrajectoryThread_8hpp_source.html
new file mode 100644
index 000000000..4970461aa
--- /dev/null
+++ b/TrajectoryThread_8hpp_source.html
@@ -0,0 +1,137 @@
+
+
+
+
+
+
+
+kinematics-dynamics: examples/cpp/exampleScrewTheoryTrajectory/TrajectoryThread.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __TRAJECTORY_THREAD_HPP__
+
4 #define __TRAJECTORY_THREAD_HPP__
+
+
6 #include <yarp/os/PeriodicThread.h>
+
+
8 #include <yarp/dev/IEncoders.h>
+
9 #include <yarp/dev/IPositionDirect.h>
+
+
11 #include <kdl/trajectory.hpp>
+
+
13 #include <ScrewTheoryIkProblem.hpp>
+
14 #include <ConfigurationSelector.hpp>
+
+
+
+
+
+
20 yarp::dev::IPositionDirect * iPosDirect,
+
+
+
23 KDL::Trajectory * trajectory,
+
+
25 : yarp::os::PeriodicThread(period * 0.001, yarp::os::PeriodicThreadClock::Absolute),
+
+
27 iPosDirect(iPosDirect),
+
+
+
30 trajectory(trajectory),
+
+
+
+
+
+
36 bool threadInit()
override ;
+
+
+
+
40 yarp::dev::IEncoders * iEncoders;
+
41 yarp::dev::IPositionDirect * iPosDirect;
+
+
+
44 KDL::Trajectory * trajectory;
+
+
+
+
+
+
Definition: TrajectoryThread.hpp:17
+
Abstract base class for a robot configuration strategy selector.
Definition: ConfigurationSelector.hpp:20
+
Proxy IK problem solver class that iterates over a sequence of subproblems.
Definition: ScrewTheoryIkProblem.hpp:95
+
+
+
+
+
diff --git a/WiimoteSensorDevice_8hpp_source.html b/WiimoteSensorDevice_8hpp_source.html
new file mode 100644
index 000000000..a2867fb46
--- /dev/null
+++ b/WiimoteSensorDevice_8hpp_source.html
@@ -0,0 +1,146 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/streamingDeviceController/WiimoteSensorDevice.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
1 #ifndef __WIIMOTE_SENSOR_DEVICE_HPP__
+
2 #define __WIIMOTE_SENSOR_DEVICE_HPP__
+
+
4 #include "StreamingDevice.hpp"
+
+
+
+
8 #include <yarp/os/Value.h>
+
9 #include <yarp/dev/IAnalogSensor.h>
+
+
11 #define DEFAULT_STEP 0.01
+
+
+
+
+
+
+
+
+
+
+
+
30 bool initialize (
bool usingStreamingPreset)
override ;
+
+
+
+
+
+
+
+
+
+
+
+
+
43 enum cmd_mode { NONE, FWD, BKWD, ROT };
+
+
45 yarp::dev::IAnalogSensor * iAnalogSensor;
+
+
+
+
49 std::vector<double> buffer;
+
+
+
+
+
+
+
+
+
Abstract class for a YARP streaming device.
Definition: StreamingDevice.hpp:46
+
Represents a Wiimote device wrapped as an analog sensor by YARP.
Definition: WiimoteSensorDevice.hpp:23
+
WiimoteSensorDevice(yarp::os::Searchable &config, bool usingPose)
Constructor.
Definition: WiimoteSensorDevice.cpp:13
+
void sendMovementCommand(double timestamp) override
Sends movement command to the cartesian controller.
Definition: WiimoteSensorDevice.cpp:132
+
bool hasValidMovementData() const override
Checks whether the device may forward acquired and processed data to the controller.
Definition: WiimoteSensorDevice.cpp:127
+
bool transformData(double scaling) override
Performs required operations on stored data.
Definition: WiimoteSensorDevice.cpp:87
+
bool initialize(bool usingStreamingPreset) override
Perform any custom initialization needed. This method is called after the successful creation of the ...
Definition: WiimoteSensorDevice.cpp:38
+
bool acquireInterfaces() override
Acquires plugin interfaces.
Definition: WiimoteSensorDevice.cpp:25
+
void stopMotion() override
Sends a movement command that would stop motion.
Definition: WiimoteSensorDevice.cpp:162
+
bool acquireData() override
Acquires data from remote device.
Definition: WiimoteSensorDevice.cpp:66
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
+
+
+
+
diff --git a/YarpTinyMath_8hpp_source.html b/YarpTinyMath_8hpp_source.html
new file mode 100644
index 000000000..480871174
--- /dev/null
+++ b/YarpTinyMath_8hpp_source.html
@@ -0,0 +1,126 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpTinyMathLib/YarpTinyMath.hpp Source File
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
3 #ifndef __YARP_TINY_MATH_HPP__
+
4 #define __YARP_TINY_MATH_HPP__
+
+
6 #define _USE_MATH_DEFINES
+
+
+
9 #include <yarp/sig/Vector.h>
+
10 #include <yarp/sig/Matrix.h>
+
+
+
+
+
26 double toDeg (
const double inRad);
+
+
33 double toRad (
const double inDeg);
+
+
40 void xUpdateH (
const yarp::sig::Vector &x, yarp::sig::Matrix &H);
+
+
42 yarp::sig::Matrix rotX(
const double &inDeg);
+
43 yarp::sig::Matrix rotY(
const double &inDeg);
+
44 yarp::sig::Matrix rotZ(
const double &inDeg);
+
+
51 yarp::sig::Matrix
eulerZYZtoH (
const yarp::sig::Vector &x,
const yarp::sig::Vector &o);
+
+
59 yarp::sig::Matrix
eulerYZtoH (
const yarp::sig::Vector &x,
const yarp::sig::Vector &o);
+
+
67 yarp::sig::Matrix
axisAngleToH (
const yarp::sig::Vector &x,
const yarp::sig::Vector &o);
+
+
+
+
+
+
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
+
yarp::sig::Matrix axisAngleToH(const yarp::sig::Vector &x, const yarp::sig::Vector &o)
Definition: YarpTinyMath.cpp:99
+
double toDeg(const double inRad)
Definition: YarpTinyMath.cpp:9
+
void xUpdateH(const yarp::sig::Vector &x, yarp::sig::Matrix &H)
Definition: YarpTinyMath.cpp:23
+
yarp::sig::Matrix eulerZYZtoH(const yarp::sig::Vector &x, const yarp::sig::Vector &o)
Definition: YarpTinyMath.cpp:77
+
double toRad(const double inDeg)
Definition: YarpTinyMath.cpp:16
+
yarp::sig::Matrix eulerYZtoH(const yarp::sig::Vector &x, const yarp::sig::Vector &o)
Definition: YarpTinyMath.cpp:88
+
+
+
+
+
diff --git a/annotated.html b/annotated.html
new file mode 100644
index 000000000..281f93bce
--- /dev/null
+++ b/annotated.html
@@ -0,0 +1,153 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here are the classes, structs, unions and interfaces with brief descriptions:
+
+
+
+
+
diff --git a/bc_s.png b/bc_s.png
new file mode 100644
index 000000000..224b29aa9
Binary files /dev/null and b/bc_s.png differ
diff --git a/bdwn.png b/bdwn.png
new file mode 100644
index 000000000..940a0b950
Binary files /dev/null and b/bdwn.png differ
diff --git a/citelist.html b/citelist.html
new file mode 100644
index 000000000..b4d5c2fdc
--- /dev/null
+++ b/citelist.html
@@ -0,0 +1,100 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Bibliography
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ [1]
+Bartek Lukawski, Ignacio Montesino Valle, Juan G. Victores, Alberto Jardón, and Carlos Balaguer. An inverse kinematics problem solver based on screw theory for manipulator arms. In XLIII Jornadas de Automática , pages 864–869. Universidade da Coruña, 2022.
+
+
+ [2]
+Bartek Lukawski, Juan G. Victores, and Carlos Balaguer. A generic controller for teleoperation on robotic manipulators using low-cost devices. In XLIV Jornadas de Automática , pages 785–788. Universidade da Coruña, 2023.
+
+
+ [3]
+Edwin Daniel Oña, Bartek Lukawski, Alberto Jardón, and Carlos Balaguer. A modular framework to facilitate the control of an assistive robotic arm using visual servoing and proximity sensing. In IEEE Int. Conf. on Autonomous Robot Systems and Competitions (ICARSC) , pages 28–33, 2020.
+
+
+ [4]
+José Manuel Pardos-Gotor. Screw Theory for Robotics - A practical approach for Modern Robot KINEMATICS - An Illustrated Handbook . Amazon Fulfillment, 9 2018. ISBN 978-1-7179-3181-8.
+
+
+
+
+
+
+
+
+
diff --git a/classOrderOneTraj-members.html b/classOrderOneTraj-members.html
new file mode 100644
index 000000000..abce18511
--- /dev/null
+++ b/classOrderOneTraj-members.html
@@ -0,0 +1,98 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for OrderOneTraj , including all inherited members.
+
+
+
+
+
diff --git a/classOrderOneTraj.html b/classOrderOneTraj.html
new file mode 100644
index 000000000..aaf58d04f
--- /dev/null
+++ b/classOrderOneTraj.html
@@ -0,0 +1,145 @@
+
+
+
+
+
+
+
+kinematics-dynamics: OrderOneTraj Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Generates a 1DOF order-one trajectory.
+
+
+
#include <TrajGen.hpp >
+
+
+
+
+
+
+
+
+
+
+
+bool configure (const double xi, const double xf, const double _T)
+
+
+bool configure (const double xi, const double xf, const double xdoti, const double xdotf, const double _T)
+
+
+double get (const double ti) const
+
+
+double getdot (const double ti) const
+
+
+double getdotdot (const double ti) const
+
+
+bool maxVelBelow (const double thresVel) const
+
+
+bool maxAccBelow (const double thresAcc) const
+
+
+double getT () const
+
+
+void dump (double samples) const
+
+
+
+
+double T
+
+
+double m
+
+
+double b
+
+
+
The documentation for this class was generated from the following file:
+
+
+
+
+
diff --git a/classOrderOneTraj.png b/classOrderOneTraj.png
new file mode 100644
index 000000000..d0ac9e1c6
Binary files /dev/null and b/classOrderOneTraj.png differ
diff --git a/classOrderThreeTraj-members.html b/classOrderThreeTraj-members.html
new file mode 100644
index 000000000..d1398d518
--- /dev/null
+++ b/classOrderThreeTraj-members.html
@@ -0,0 +1,100 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for OrderThreeTraj , including all inherited members.
+
+
+
+
+
diff --git a/classOrderThreeTraj.html b/classOrderThreeTraj.html
new file mode 100644
index 000000000..2da43aef1
--- /dev/null
+++ b/classOrderThreeTraj.html
@@ -0,0 +1,452 @@
+
+
+
+
+
+
+
+kinematics-dynamics: OrderThreeTraj Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Generates a 1DOF order-three trajectory.
+
+
+
#include <TrajGen.hpp >
+
+
+
+
+
+
+
+
+
+
+bool configure (const double xi, const double xf, const double _T)
+
+bool configure (const double xi, const double xf, const double xdoti, const double xdotf, const double _T)
+
+double get (const double ti) const
+
+double getdot (const double ti) const
+
+double getdotdot (const double ti) const
+
+bool maxVelBelow (const double thresVel) const
+
+bool maxAccBelow (const double thresAcc) const
+
+double getT () const
+
+void dump (double samples) const
+
+
+
+
+double a3
+
+
+double a2
+
+
+double a1
+
+
+double a0
+
+
+double T
+
+
+
+
+
◆ configure() [1/2]
+
+
+
+
+
+
+
+
+ bool OrderThreeTraj::configure
+ (
+ const double
+ xi ,
+
+
+
+
+ const double
+ xf ,
+
+
+
+
+ const double
+ _T
+
+
+
+ )
+
+
+
+
+
+inline virtual
+
+
+
+
Configure the trajectory. Forces null initial and final velocities.
+
+
Implements Traj .
+
+
+
+
+
◆ configure() [2/2]
+
+
+
+
+
+
+
+
+ bool OrderThreeTraj::configure
+ (
+ const double
+ xi ,
+
+
+
+
+ const double
+ xf ,
+
+
+
+
+ const double
+ xdoti ,
+
+
+
+
+ const double
+ xdotf ,
+
+
+
+
+ const double
+ _T
+
+
+
+ )
+
+
+
+
+
+inline virtual
+
+
+
+
Configure the trajectory setting an initial and final velocity too (warning: possible overshoot).
+
+
Implements Traj .
+
+
+
+
+
◆ dump()
+
+
+
+
+
+
+
+
+ void OrderThreeTraj::dump
+ (
+ double
+ samples )
+ const
+
+
+
+
+inline virtual
+
+
+
+
Printf of ti, f(ti), fdot(ti), fdotdot(ti) for whole duration interval.
Parameters
+
+ samples number of lines to print.
+
+
+
+
+
Implements Traj .
+
+
+
+
+
◆ get()
+
+
+
+
+
+
+
+
+ double OrderThreeTraj::get
+ (
+ const double
+ ti )
+ const
+
+
+
+
+inline virtual
+
+
+
+
Returns the value of the function at instant ti.
+
+
Implements Traj .
+
+
+
+
+
◆ getdot()
+
+
+
+
+
+
+
+
+ double OrderThreeTraj::getdot
+ (
+ const double
+ ti )
+ const
+
+
+
+
+inline virtual
+
+
+
+
Returns the value of the first derivative of the function at instant ti.
+
+
Implements Traj .
+
+
+
+
+
◆ getdotdot()
+
+
+
+
+
+
+
+
+ double OrderThreeTraj::getdotdot
+ (
+ const double
+ ti )
+ const
+
+
+
+
+inline virtual
+
+
+
+
Returns the value of the second derivative of the function at instant ti.
+
+
Implements Traj .
+
+
+
+
+
◆ getT()
+
+
+
+
+
+
+
+
+ double OrderThreeTraj::getT
+ (
+ )
+ const
+
+
+
+
+inline virtual
+
+
+
+
Returns duration assigned to the trajectory instance.
+
+
Implements Traj .
+
+
+
+
+
◆ maxAccBelow()
+
+
+
+
+
+
+
+
+ bool OrderThreeTraj::maxAccBelow
+ (
+ const double
+ thresAcc )
+ const
+
+
+
+
+inline virtual
+
+
+
+
Check if the maximum of the second derivative is below a certain threshold.
+
+
Implements Traj .
+
+
+
+
+
◆ maxVelBelow()
+
+
+
+
+
+
+
+
+ bool OrderThreeTraj::maxVelBelow
+ (
+ const double
+ thresVel )
+ const
+
+
+
+
+inline virtual
+
+
+
+
Check if the maximum of the first derivative is below a certain threshold.
+
+
Implements Traj .
+
+
+
+
The documentation for this class was generated from the following file:
+
+
+
+
+
diff --git a/classOrderThreeTraj.png b/classOrderThreeTraj.png
new file mode 100644
index 000000000..9a2fb75f1
Binary files /dev/null and b/classOrderThreeTraj.png differ
diff --git a/classTraj-members.html b/classTraj-members.html
new file mode 100644
index 000000000..cc0b48279
--- /dev/null
+++ b/classTraj-members.html
@@ -0,0 +1,94 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for Traj , including all inherited members.
+
+ configure (const double xi, const double xf, const double _T)=0 (defined in Traj )Traj pure virtual
+ configure (const double xi, const double xf, const double xdoti, const double xdotf, const double _T)=0 (defined in Traj )Traj pure virtual
+ dump (double samples) const =0 (defined in Traj )Traj pure virtual
+ get (const double ti) const =0 (defined in Traj )Traj pure virtual
+ getdot (const double ti) const =0 (defined in Traj )Traj pure virtual
+ getdotdot (const double ti) const =0 (defined in Traj )Traj pure virtual
+ getT () const =0 (defined in Traj )Traj pure virtual
+ maxAccBelow (const double thresAcc) const =0 (defined in Traj )Traj pure virtual
+ maxVelBelow (const double thresVel) const =0 (defined in Traj )Traj pure virtual
+ ~Traj () (defined in Traj )Traj inline virtual
+
+
+
+
+
diff --git a/classTraj.html b/classTraj.html
new file mode 100644
index 000000000..8bbc0d86f
--- /dev/null
+++ b/classTraj.html
@@ -0,0 +1,133 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Traj Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
A base class for 1 degree-of-freedom trajectories.
+
+
+
#include <TrajGen.hpp >
+
+
+
+
+
+
+
+
+
+
+
+
+virtual bool configure (const double xi, const double xf, const double _T)=0
+
+
+virtual bool configure (const double xi, const double xf, const double xdoti, const double xdotf, const double _T)=0
+
+
+virtual double get (const double ti) const =0
+
+
+virtual double getdot (const double ti) const =0
+
+
+virtual double getdotdot (const double ti) const =0
+
+
+virtual bool maxVelBelow (const double thresVel) const =0
+
+
+virtual bool maxAccBelow (const double thresAcc) const =0
+
+
+virtual double getT () const =0
+
+
+virtual void dump (double samples) const =0
+
+
+
The documentation for this class was generated from the following file:
+
+
+
+
+
diff --git a/classTraj.png b/classTraj.png
new file mode 100644
index 000000000..8a1768cc6
Binary files /dev/null and b/classTraj.png differ
diff --git a/classTrajectoryThread-members.html b/classTrajectoryThread-members.html
new file mode 100644
index 000000000..c20c72beb
--- /dev/null
+++ b/classTrajectoryThread-members.html
@@ -0,0 +1,94 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for TrajectoryThread , including all inherited members.
+
+
+
+
+
diff --git a/classTrajectoryThread.html b/classTrajectoryThread.html
new file mode 100644
index 000000000..c7d9ee944
--- /dev/null
+++ b/classTrajectoryThread.html
@@ -0,0 +1,136 @@
+
+
+
+
+
+
+
+kinematics-dynamics: TrajectoryThread Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+bool threadInit () override
+
+
+void run () override
+
+
+
The documentation for this class was generated from the following files:
+examples/cpp/exampleScrewTheoryTrajectory/TrajectoryThread.hpp
+examples/cpp/exampleScrewTheoryTrajectory/TrajectoryThread.cpp
+
+
+
+
+
+
diff --git a/classTrajectoryThread.png b/classTrajectoryThread.png
new file mode 100644
index 000000000..4bdc8afe1
Binary files /dev/null and b/classTrajectoryThread.png differ
diff --git a/classes.html b/classes.html
new file mode 100644
index 000000000..7b2454240
--- /dev/null
+++ b/classes.html
@@ -0,0 +1,132 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Index
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/classroboticslab_1_1AsibotConfiguration-members.html b/classroboticslab_1_1AsibotConfiguration-members.html
new file mode 100644
index 000000000..d824a12e1
--- /dev/null
+++ b/classroboticslab_1_1AsibotConfiguration-members.html
@@ -0,0 +1,102 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::AsibotConfiguration , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1AsibotConfiguration.html b/classroboticslab_1_1AsibotConfiguration.html
new file mode 100644
index 000000000..48b63e1a1
--- /dev/null
+++ b/classroboticslab_1_1AsibotConfiguration.html
@@ -0,0 +1,365 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::AsibotConfiguration Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Abstract base class for a robot configuration strategy selector.
+ More...
+
+
#include <AsibotConfiguration.hpp >
+
+
+
+
+
+
+
+
+
+
+struct Pose
+ Helper class to store a specific robot configuration. More...
+
+
+
+
+using JointsIn = const std::vector< double > &
+ Const vector of joint angles (input parameter).
+
+
+using JointsOut = std::vector< double > &
+ Vector of joint angles (output parameter).
+
+
+
+
Designed with ASIBOT's specific case in mind, which entails up to four different configurations depending on initial angles provided.
+
+
+
◆ AsibotConfiguration()
+
+
+
+
+
+
+
+
+ roboticslab::AsibotConfiguration::AsibotConfiguration
+ (
+ JointsIn
+ qMin ,
+
+
+
+
+ JointsIn
+ qMax
+
+
+
+ )
+
+
+
+
+
+inline
+
+
+
+
Parameters
+
+ qMin vector of minimum joint limits [deg]
+ qMax vector of maximum joint limits [deg]
+
+
+
+
+
+
+
+
+
◆ configure()
+
+
+
+
+
+
+
+
+ bool AsibotConfiguration::configure
+ (
+ double
+ q1 ,
+
+
+
+
+ double
+ q2u ,
+
+
+
+
+ double
+ q2d ,
+
+
+
+
+ double
+ q3 ,
+
+
+
+
+ double
+ q4u ,
+
+
+
+
+ double
+ q4d ,
+
+
+
+
+ double
+ q5
+
+
+
+ )
+
+
+
+
+
+virtual
+
+
+
+
Distinguishes between elbow up and down poses. Make sure that:
+
oyP = q2u + q3 + q4u = q2d - q3 + q4d
+
Parameters
+
+ q1 IK solution for joint 1 [deg]
+ q2u IK solution for joint 2 (elbow up) [deg]
+ q2d IK solution for joint 2 (elbow down) [deg]
+ q3 IK solution for joint 3 [deg]
+ q4u IK solution for joint 4 (elbow up) [deg]
+ q4d IK solution for joint 4 (elbow down) [deg]
+ q5 IK solution for joint 5 [deg]
+
+
+
+
Returns true/false on success/failure
+
+
+
+
+
◆ findOptimalConfiguration()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::AsibotConfiguration::findOptimalConfiguration
+ (
+ JointsIn
+ qGuess )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ retrieveAngles()
+
+
+
+
+
+
+
+
+ virtual void roboticslab::AsibotConfiguration::retrieveAngles
+ (
+ JointsOut
+ q )
+ const
+
+
+
+
+inline virtual
+
+
+
+
Parameters
+
+ q vector of joint angles [deg]
+
+
+
+
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/AsibotSolver/AsibotConfiguration.hpp
+libraries/YarpPlugins/AsibotSolver/AsibotConfiguration.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1AsibotConfiguration.png b/classroboticslab_1_1AsibotConfiguration.png
new file mode 100644
index 000000000..a9f07d225
Binary files /dev/null and b/classroboticslab_1_1AsibotConfiguration.png differ
diff --git a/classroboticslab_1_1AsibotConfigurationFactory-members.html b/classroboticslab_1_1AsibotConfigurationFactory-members.html
new file mode 100644
index 000000000..3cbbc304d
--- /dev/null
+++ b/classroboticslab_1_1AsibotConfigurationFactory-members.html
@@ -0,0 +1,93 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::AsibotConfigurationFactory , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1AsibotConfigurationFactory.html b/classroboticslab_1_1AsibotConfigurationFactory.html
new file mode 100644
index 000000000..44dfcfc6f
--- /dev/null
+++ b/classroboticslab_1_1AsibotConfigurationFactory.html
@@ -0,0 +1,204 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::AsibotConfigurationFactory Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Base factory class for AsibotConfiguration .
+ More...
+
+
#include <AsibotConfiguration.hpp >
+
+
+
+
+
+
+
+
+
+
+
Acts as the base class in the abstract factory pattern , encapsulates individual factories.
+
+
+
◆ AsibotConfigurationFactory()
+
+
+
+
+
+
+
+
+
+inline protected
+
+
+
+
Parameters
+
+ qMin vector of minimum joint limits [deg]
+ qMax vector of maximum joint limits [deg]
+
+
+
+
+
+
+
+
+
◆ create()
+
+
+
+
+
+
+
+
+ virtual AsibotConfiguration * roboticslab::AsibotConfigurationFactory::create
+ (
+ )
+ const
+
+
+
+
+pure virtual
+
+
+
+
+
The documentation for this class was generated from the following file:
+
+
+
+
+
diff --git a/classroboticslab_1_1AsibotConfigurationFactory.png b/classroboticslab_1_1AsibotConfigurationFactory.png
new file mode 100644
index 000000000..2c7093cbe
Binary files /dev/null and b/classroboticslab_1_1AsibotConfigurationFactory.png differ
diff --git a/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacement-members.html b/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacement-members.html
new file mode 100644
index 000000000..b7698bcfe
--- /dev/null
+++ b/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacement-members.html
@@ -0,0 +1,104 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::AsibotConfigurationLeastOverallAngularDisplacement , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacement.html b/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacement.html
new file mode 100644
index 000000000..4d7e95d71
--- /dev/null
+++ b/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacement.html
@@ -0,0 +1,254 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::AsibotConfigurationLeastOverallAngularDisplacement Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
IK solver configuration strategy selector based on the overall angle displacement of all joints.
+ More...
+
+
#include <AsibotConfiguration.hpp >
+
+
+
+
+
+
+
+
+
+
+
+std::vector< double > getDiffs (JointsIn qGuess, const Pose &pose)
+ Obtains vector of differences between current and desired joint angles [deg].
+
+
+
+
+
+using JointsIn = const std::vector< double > &
+ Const vector of joint angles (input parameter).
+
+
+using JointsOut = std::vector< double > &
+ Vector of joint angles (output parameter).
+
+
+
+JointsIn _qMin
+
+
+JointsIn _qMax
+
+
+Pose optimalPose
+
+
+Pose forwardElbowUp
+
+
+Pose forwardElbowDown
+
+
+Pose reversedElbowUp
+
+
+Pose reversedElbowDown
+
+
+
+
Selects the configuration that entails the lowest sum of angular displacements across all joints. Upon choosing between elbow up and down alternatives, a small angular travel distance for joint 2 is preferred.
+
+
+
◆ AsibotConfigurationLeastOverallAngularDisplacement()
+
+
+
+
+
+
+
+
+ roboticslab::AsibotConfigurationLeastOverallAngularDisplacement::AsibotConfigurationLeastOverallAngularDisplacement
+ (
+ JointsIn
+ qMin ,
+
+
+
+
+ JointsIn
+ qMax
+
+
+
+ )
+
+
+
+
+
+inline
+
+
+
+
Parameters
+
+ qMin vector of minimum joint limits [deg]
+ qMax vector of maximum joint limits [deg]
+
+
+
+
+
+
+
+
+
◆ findOptimalConfiguration()
+
+
+
+
+
+
+
+
+ bool AsibotConfigurationLeastOverallAngularDisplacement::findOptimalConfiguration
+ (
+ JointsIn
+ qGuess )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ qGuess vector of joint angles for current robot position [deg]
+
+
+
+
Returns true/false on success/failure
+
+
Implements roboticslab::AsibotConfiguration .
+
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/AsibotSolver/AsibotConfiguration.hpp
+libraries/YarpPlugins/AsibotSolver/AsibotConfigurationLeastOverallAngularDisplacement.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacement.png b/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacement.png
new file mode 100644
index 000000000..f692715b5
Binary files /dev/null and b/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacement.png differ
diff --git a/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacementFactory-members.html b/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacementFactory-members.html
new file mode 100644
index 000000000..2a2ff9ded
--- /dev/null
+++ b/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacementFactory-members.html
@@ -0,0 +1,94 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::AsibotConfigurationLeastOverallAngularDisplacementFactory , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacementFactory.html b/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacementFactory.html
new file mode 100644
index 000000000..625866907
--- /dev/null
+++ b/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacementFactory.html
@@ -0,0 +1,204 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::AsibotConfigurationLeastOverallAngularDisplacementFactory Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Implementation factory class for AsibotConfigurationLeastOverallAngularDisplacement .
+ More...
+
+
#include <AsibotConfiguration.hpp >
+
+
+
+
+
+
+
+
+
+
+
+
+
◆ AsibotConfigurationLeastOverallAngularDisplacementFactory()
+
+
+
+
Parameters
+
+ qMin vector of minimum joint limits [deg]
+ qMax vector of maximum joint limits [deg]
+
+
+
+
+
+
+
+
+
◆ create()
+
+
+
+
+
+
+
+
+ AsibotConfiguration * roboticslab::AsibotConfigurationLeastOverallAngularDisplacementFactory::create
+ (
+ )
+ const
+
+
+
+
+inline override virtual
+
+
+
+
+
The documentation for this class was generated from the following file:
+
+
+
+
+
diff --git a/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacementFactory.png b/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacementFactory.png
new file mode 100644
index 000000000..3f989fe98
Binary files /dev/null and b/classroboticslab_1_1AsibotConfigurationLeastOverallAngularDisplacementFactory.png differ
diff --git a/classroboticslab_1_1AsibotSolver-members.html b/classroboticslab_1_1AsibotSolver-members.html
new file mode 100644
index 000000000..3341db07e
--- /dev/null
+++ b/classroboticslab_1_1AsibotSolver-members.html
@@ -0,0 +1,118 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::AsibotSolver , including all inherited members.
+
+ A0 (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver private
+ A1 (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver private
+ A2 (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver private
+ A3 (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver private
+ appendLink (const std::vector< double > &x) overrideroboticslab::AsibotSolver virtual
+ BASE_FRAME enum valueroboticslab::ICartesianSolver
+ changeOrigin (const std::vector< double > &x_old_obj, const std::vector< double > &x_new_old, std::vector< double > &x_new_obj) overrideroboticslab::AsibotSolver virtual
+ close () override (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver
+ confFactory (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver private
+ diffInvKin (const std::vector< double > &q, const std::vector< double > &xdot, std::vector< double > &qdot, reference_frame frame) overrideroboticslab::AsibotSolver virtual
+ fwdKin (const std::vector< double > &q, std::vector< double > &x) overrideroboticslab::AsibotSolver virtual
+ getConfiguration () const (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver private
+ getNumJoints () overrideroboticslab::AsibotSolver virtual
+ getNumTcps () overrideroboticslab::AsibotSolver virtual
+ getTcpFrame () const (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver private
+ invDyn (const std::vector< double > &q, std::vector< double > &t) overrideroboticslab::AsibotSolver virtual
+ invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< double > &ftip, std::vector< double > &t, reference_frame frame) overrideroboticslab::AsibotSolver virtual
+ roboticslab::ICartesianSolver::invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< std::vector< double >> &fexts, std::vector< double > &t)roboticslab::ICartesianSolver inline virtual
+ invKin (const std::vector< double > &xd, const std::vector< double > &qGuess, std::vector< double > &q, reference_frame frame) overrideroboticslab::AsibotSolver virtual
+ mtx (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver mutable private
+ open (yarp::os::Searchable &config) override (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver
+ poseDiff (const std::vector< double > &xLhs, const std::vector< double > &xRhs, std::vector< double > &xOut) overrideroboticslab::AsibotSolver virtual
+ qMax (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver private
+ qMin (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver private
+ reference_frame enum nameroboticslab::ICartesianSolver
+ restoreOriginalChain () overrideroboticslab::AsibotSolver virtual
+ setTcpFrame (const AsibotTcpFrame &tcpFrameStruct) (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver private
+ TCP_FRAME enum valueroboticslab::ICartesianSolver
+ tcpFrameStruct (defined in roboticslab::AsibotSolver )roboticslab::AsibotSolver private
+ ~ICartesianSolver ()=defaultroboticslab::ICartesianSolver virtual
+
+
+
+
+
diff --git a/classroboticslab_1_1AsibotSolver.html b/classroboticslab_1_1AsibotSolver.html
new file mode 100644
index 000000000..40601ced1
--- /dev/null
+++ b/classroboticslab_1_1AsibotSolver.html
@@ -0,0 +1,732 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::AsibotSolver Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
The AsibotSolver implements ICartesianSolver .
+
+
+
#include <AsibotSolver.hpp >
+
+
+
+
+
+
+
+
+
+
+int getNumJoints () override
+ Get number of joints for which the solver has been configured. More...
+
+int getNumTcps () override
+ Get number of TCPs for which the solver has been configured. More...
+
+bool appendLink (const std::vector< double > &x) override
+ Append an additional link. More...
+
+bool restoreOriginalChain () override
+ Restore original kinematic chain. More...
+
+bool changeOrigin (const std::vector< double > &x_old_obj, const std::vector< double > &x_new_old, std::vector< double > &x_new_obj) override
+ Change origin in which a pose is expressed. More...
+
+bool fwdKin (const std::vector< double > &q, std::vector< double > &x) override
+ Perform forward kinematics. More...
+
+bool poseDiff (const std::vector< double > &xLhs, const std::vector< double > &xRhs, std::vector< double > &xOut) override
+ Obtain difference between supplied pose inputs. More...
+
+bool invKin (const std::vector< double > &xd, const std::vector< double > &qGuess, std::vector< double > &q, reference_frame frame) override
+ Perform inverse kinematics. More...
+
+bool diffInvKin (const std::vector< double > &q, const std::vector< double > &xdot, std::vector< double > &qdot, reference_frame frame) override
+ Perform differential inverse kinematics. More...
+
+bool invDyn (const std::vector< double > &q, std::vector< double > &t) override
+ Perform inverse dynamics. More...
+
+bool invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< double > &ftip, std::vector< double > &t, reference_frame frame) override
+ Perform inverse dynamics. More...
+
+
+bool open (yarp::os::Searchable &config) override
+
+
+bool close () override
+
+
+
+virtual ~ICartesianSolver ()=default
+ Destructor.
+
+virtual bool invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< std::vector< double >> &fexts, std::vector< double > &t)
+ Perform inverse dynamics. More...
+
+
+
+
+double A0
+
+
+double A1
+
+
+double A2
+
+
+double A3
+
+
+std::vector< double > qMin
+
+
+std::vector< double > qMax
+
+
+AsibotConfigurationFactory * confFactory {nullptr}
+
+
+AsibotTcpFrame tcpFrameStruct
+
+
+std::mutex mtx
+
+
+
+
+
◆ appendLink()
+
+
+
+
+
+
+
+
+ bool AsibotSolver::appendLink
+ (
+ const std::vector< double > &
+ x )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ x 6-element vector describing end-effector frame in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ changeOrigin()
+
+
+
+
+
+
+
+
+ bool AsibotSolver::changeOrigin
+ (
+ const std::vector< double > &
+ x_old_obj ,
+
+
+
+
+ const std::vector< double > &
+ x_new_old ,
+
+
+
+
+ std::vector< double > &
+ x_new_obj
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ x_old_obj_in 6-element vector describing a pose in cartesian space, expressed in the old frame; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ x_new_old 6-element vector describing a transformation from the new to the old frame; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ x_new_obj 6-element vector describing a pose in cartesian space, expressed in the new frame; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ diffInvKin()
+
+
+
+
+
+
+
+
+ bool AsibotSolver::diffInvKin
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ const std::vector< double > &
+ xdot ,
+
+
+
+
+ std::vector< double > &
+ qdot ,
+
+
+
+
+ reference_frame
+ frame
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ q Vector describing current position in joint space (meters or degrees).
+ xdot 6-element vector describing desired velocity in cartesian space; first three elements denote translational velocity (meters/second), last three denote angular velocity (radians/second).
+ qdot Vector describing target velocity in joint space (meters/second or degrees/second).
+ frame Points at the reference_frame the desired position is expressed in.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ fwdKin()
+
+
+
+
+
+
+
+
+ bool AsibotSolver::fwdKin
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ std::vector< double > &
+ x
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ q Vector describing a position in joint space (meters or degrees).
+ x 6-element vector describing same position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ getNumJoints()
+
+
+
+
+
+
+
+
+ int AsibotSolver::getNumJoints
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ getNumTcps()
+
+
+
+
+
+
+
+
+ int AsibotSolver::getNumTcps
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ invDyn() [1/2]
+
+
+
+
+
+
+
+
+ bool AsibotSolver::invDyn
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ const std::vector< double > &
+ qdot ,
+
+
+
+
+ const std::vector< double > &
+ qdotdot ,
+
+
+
+
+ const std::vector< double > &
+ ftip ,
+
+
+
+
+ std::vector< double > &
+ t ,
+
+
+
+
+ reference_frame
+ frame
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ q Vector describing current position in joint space (meters or degrees).
+ qdot Vector describing current velocity in joint space (meters/second or degrees/second).
+ qdotdot Vector describing current acceleration in joint space (meters/second² or degrees/second²).
+ ftip Vector describing an external force applied to the robot tip, expressed in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+ t 6-element vector describing desired forces in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+ frame Points at the reference_frame ftip
is expressed in.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ invDyn() [2/2]
+
+
+
+
+
+
+
+
+ bool AsibotSolver::invDyn
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ std::vector< double > &
+ t
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Assumes null joint velocities and accelerations, and no external forces.
+
Parameters
+
+ q Vector describing current position in joint space (meters or degrees).
+ t 6-element vector describing desired forces in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ invKin()
+
+
+
+
+
+
+
+
+ bool AsibotSolver::invKin
+ (
+ const std::vector< double > &
+ xd ,
+
+
+
+
+ const std::vector< double > &
+ qGuess ,
+
+
+
+
+ std::vector< double > &
+ q ,
+
+
+
+
+ reference_frame
+ frame
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ xd 6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ qGuess Vector describing current position in joint space (meters or degrees).
+ q Vector describing target position in joint space (meters or degrees).
+ frame Points at the reference_frame the desired position is expressed in.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ poseDiff()
+
+
+
+
+
+
+
+
+ bool AsibotSolver::poseDiff
+ (
+ const std::vector< double > &
+ xLhs ,
+
+
+
+
+ const std::vector< double > &
+ xRhs ,
+
+
+
+
+ std::vector< double > &
+ xOut
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
The result is an infinitesimal displacement twist, i.e. a vector, for which the operation of addition makes physical sense.
+
Parameters
+
+ xLhs 6-element vector describing a pose in cartesian space (left hand side); first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ xRhs 6-element vector describing a pose in cartesian space (right hand side); first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ xOut 6-element vector describing a pose in cartesian space (result); first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ restoreOriginalChain()
+
+
+
+
+
+
+
+
+ bool AsibotSolver::restoreOriginalChain
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/AsibotSolver/AsibotSolver.hpp
+libraries/YarpPlugins/AsibotSolver/AsibotSolver.cpp
+libraries/YarpPlugins/AsibotSolver/DeviceDriverImpl.cpp
+libraries/YarpPlugins/AsibotSolver/ICartesianSolverImpl.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1AsibotSolver.png b/classroboticslab_1_1AsibotSolver.png
new file mode 100644
index 000000000..8c4fa0375
Binary files /dev/null and b/classroboticslab_1_1AsibotSolver.png differ
diff --git a/classroboticslab_1_1BasicCartesianControl-members.html b/classroboticslab_1_1BasicCartesianControl-members.html
new file mode 100644
index 000000000..72a4ea066
--- /dev/null
+++ b/classroboticslab_1_1BasicCartesianControl-members.html
@@ -0,0 +1,161 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::BasicCartesianControl , including all inherited members.
+
+ act (int command) overrideroboticslab::BasicCartesianControl virtual
+ BasicCartesianControl () (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl inline
+ checkControlModes (int mode) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ checkJointLimits (const std::vector< double > &q) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ checkJointLimits (const std::vector< double > &q, const std::vector< double > &qdot) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ checkJointVelocities (const std::vector< double > &qdot) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ close () override (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl
+ cmcPeriodMs (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ cmcSuccess (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ computeIsocronousSpeeds (const std::vector< double > &q, const std::vector< double > &qd, std::vector< double > &qdot) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ currentState (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ doFailFastChecks (const std::vector< double > &initialQ) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ duration (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ enableFailFast (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ encoderErrors (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ fd roboticslab::BasicCartesianControl private
+ forc (const std::vector< double > &fd) overrideroboticslab::BasicCartesianControl virtual
+ gain (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ gcmp () overrideroboticslab::BasicCartesianControl virtual
+ getCurrentState () const (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ getParameter (int vocab, double *value) overrideroboticslab::BasicCartesianControl virtual
+ getParameters (std::map< int, double > ¶ms) overrideroboticslab::BasicCartesianControl virtual
+ handleForc (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const StateWatcher &watcher) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ handleGcmp (const std::vector< double > &q, const StateWatcher &watcher) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ handleMovj (const std::vector< double > &q, const StateWatcher &watcher) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ handleMovlPosd (const std::vector< double > &q, const StateWatcher &watcher) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ handleMovlVel (const std::vector< double > &q, const StateWatcher &watcher) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ handleMovv (const std::vector< double > &q, const StateWatcher &watcher) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ iCartesianSolver (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ iControlMode (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ iEncoders (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ inv (const std::vector< double > &xd, std::vector< double > &q) overrideroboticslab::BasicCartesianControl virtual
+ iPositionControl (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ iPositionDirect (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ iPreciselyTimed (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ iTorqueControl (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ iVelocityControl (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ movementStartTime roboticslab::BasicCartesianControl private
+ movi (const std::vector< double > &x) (defined in roboticslab::ICartesianControl )roboticslab::ICartesianControl inline virtual
+ movj (const std::vector< double > &xd) overrideroboticslab::BasicCartesianControl virtual
+ movl (const std::vector< double > &xd) overrideroboticslab::BasicCartesianControl virtual
+ movv (const std::vector< double > &xdotd) overrideroboticslab::BasicCartesianControl virtual
+ numJoints (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ open (yarp::os::Searchable &config) override (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl
+ pose (const std::vector< double > &x) overrideroboticslab::BasicCartesianControl virtual
+ presetStreamingCommand (int command) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ qdotMax (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ qdotMin (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ qMax (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ qMin (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ qRefSpeeds (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ referenceFrame (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ relj (const std::vector< double > &xd) overrideroboticslab::BasicCartesianControl virtual
+ robotDevice (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ run () override (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl
+ setControlModes (int mode) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ setCurrentState (int value) (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ setParameter (int vocab, double value) overrideroboticslab::BasicCartesianControl virtual
+ setParameters (const std::map< int, double > ¶ms) overrideroboticslab::BasicCartesianControl virtual
+ solverDevice (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ stat (std::vector< double > &x, int *state=nullptr, double *timestamp=nullptr) overrideroboticslab::BasicCartesianControl virtual
+ stateMutex (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl mutable private
+ stopControl () overrideroboticslab::BasicCartesianControl virtual
+ streamingCommand (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ tool (const std::vector< double > &x) overrideroboticslab::BasicCartesianControl virtual
+ trajectories roboticslab::BasicCartesianControl private
+ twist (const std::vector< double > &xdot) overrideroboticslab::BasicCartesianControl virtual
+ usePosdMovl (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ vmoStored roboticslab::BasicCartesianControl private
+ wait (double timeout) overrideroboticslab::BasicCartesianControl virtual
+ waitPeriodMs (defined in roboticslab::BasicCartesianControl )roboticslab::BasicCartesianControl private
+ wrench (const std::vector< double > &w) overrideroboticslab::BasicCartesianControl virtual
+ ~ICartesianControl ()=defaultroboticslab::ICartesianControl virtual
+
+
+
+
+
diff --git a/classroboticslab_1_1BasicCartesianControl.html b/classroboticslab_1_1BasicCartesianControl.html
new file mode 100644
index 000000000..85110f5d2
--- /dev/null
+++ b/classroboticslab_1_1BasicCartesianControl.html
@@ -0,0 +1,1155 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::BasicCartesianControl Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
The BasicCartesianControl class implements ICartesianControl .
+
+
+
#include <BasicCartesianControl.hpp >
+
+
+
+
+
+
+
+
+
+
+bool stat (std::vector< double > &x, int *state=nullptr, double *timestamp=nullptr) override
+ Current state and position. More...
+
+bool inv (const std::vector< double > &xd, std::vector< double > &q) override
+ Inverse kinematics. More...
+
+bool movj (const std::vector< double > &xd) override
+ Move in joint space, absolute coordinates. More...
+
+bool relj (const std::vector< double > &xd) override
+ Move in joint space, relative coordinates. More...
+
+bool movl (const std::vector< double > &xd) override
+ Linear move to target position. More...
+
+bool movv (const std::vector< double > &xdotd) override
+ Linear move with given velocity. More...
+
+bool gcmp () override
+ Gravity compensation. More...
+
+bool forc (const std::vector< double > &fd ) override
+ Force control. More...
+
+bool stopControl () override
+ Stop control. More...
+
+bool wait (double timeout) override
+ Wait until completion. More...
+
+bool tool (const std::vector< double > &x) override
+ Change tool. More...
+
+bool act (int command) override
+ Actuate tool. More...
+
+void pose (const std::vector< double > &x) override
+ Achieve pose. More...
+
+void twist (const std::vector< double > &xdot) override
+ Instantaneous velocity steps. More...
+
+void wrench (const std::vector< double > &w) override
+ Exert force. More...
+
+bool setParameter (int vocab, double value) override
+ Set a configuration parameter. More...
+
+bool getParameter (int vocab, double *value) override
+ Retrieve a configuration parameter. More...
+
+bool setParameters (const std::map< int, double > ¶ms) override
+ Set multiple configuration parameters. More...
+
+bool getParameters (std::map< int, double > ¶ms) override
+ Retrieve multiple configuration parameters. More...
+
+
+void run () override
+
+
+bool open (yarp::os::Searchable &config) override
+
+
+bool close () override
+
+
+
+virtual ~ICartesianControl ()=default
+ Destructor.
+
+
+virtual void movi (const std::vector< double > &x)
+
+
+
+
+int getCurrentState () const
+
+
+void setCurrentState (int value)
+
+
+bool checkJointLimits (const std::vector< double > &q)
+
+
+bool checkJointLimits (const std::vector< double > &q, const std::vector< double > &qdot)
+
+
+bool checkJointVelocities (const std::vector< double > &qdot)
+
+
+bool doFailFastChecks (const std::vector< double > &initialQ)
+
+
+bool checkControlModes (int mode)
+
+
+bool setControlModes (int mode)
+
+
+bool presetStreamingCommand (int command)
+
+
+bool computeIsocronousSpeeds (const std::vector< double > &q, const std::vector< double > &qd, std::vector< double > &qdot)
+
+
+void handleMovj (const std::vector< double > &q, const StateWatcher &watcher)
+
+
+void handleMovlVel (const std::vector< double > &q, const StateWatcher &watcher)
+
+
+void handleMovlPosd (const std::vector< double > &q, const StateWatcher &watcher)
+
+
+void handleMovv (const std::vector< double > &q, const StateWatcher &watcher)
+
+
+void handleGcmp (const std::vector< double > &q, const StateWatcher &watcher)
+
+
+void handleForc (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const StateWatcher &watcher)
+
+
+
+
+yarp::dev::PolyDriver solverDevice
+
+
+ICartesianSolver * iCartesianSolver {nullptr}
+
+
+yarp::dev::PolyDriver robotDevice
+
+
+yarp::dev::IControlMode * iControlMode {nullptr}
+
+
+yarp::dev::IEncoders * iEncoders {nullptr}
+
+
+yarp::dev::IPositionControl * iPositionControl {nullptr}
+
+
+yarp::dev::IPositionDirect * iPositionDirect {nullptr}
+
+
+yarp::dev::IPreciselyTimed * iPreciselyTimed {nullptr}
+
+
+yarp::dev::ITorqueControl * iTorqueControl {nullptr}
+
+
+yarp::dev::IVelocityControl * iVelocityControl {nullptr}
+
+
+ICartesianSolver::reference_frame referenceFrame
+
+
+double gain
+
+
+double duration
+
+
+int cmcPeriodMs
+
+
+int waitPeriodMs
+
+
+int numJoints
+
+
+int currentState
+
+
+int streamingCommand
+
+
+bool usePosdMovl
+
+
+bool enableFailFast
+
+
+std::mutex stateMutex
+
+std::vector< double > vmoStored
+
+double movementStartTime
+
+std::vector< std::unique_ptr< KDL::Trajectory > > trajectories
+
+std::vector< double > fd
+
+
+int encoderErrors {0}
+
+
+std::atomic_bool cmcSuccess
+
+
+std::vector< double > qMin
+
+
+std::vector< double > qMax
+
+
+std::vector< double > qdotMin
+
+
+std::vector< double > qdotMax
+
+
+std::vector< double > qRefSpeeds
+
+
+
+
+
◆ act()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::act
+ (
+ int
+ command )
+
+
+
+
+
+override virtual
+
+
+
+
Send control command to actuate the robot's tool, if available.
+
Parameters
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ forc()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::forc
+ (
+ const std::vector< double > &
+ fd )
+
+
+
+
+
+override virtual
+
+
+
+
Apply desired forces in task space.
+
Parameters
+
+ fd 6-element vector describing desired force exerted by the TCP in cartesian space; first three elements denote linear force (Newton), last three denote torque (Newton*meters).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ gcmp()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::gcmp
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ getParameter()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::getParameter
+ (
+ int
+ vocab ,
+
+
+
+
+ double *
+ value
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Ask the controller to retrieve a parameter of 'double' type.
+
Parameters
+
+ vocab YARP-encoded vocab (parameter key).
+ value Parameter value encoded as a double.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ getParameters()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::getParameters
+ (
+ std::map< int, double > &
+ params )
+
+
+
+
+
+override virtual
+
+
+
+
Ask the controller to retrieve all available parameters at once.
+
Parameters
+
+ params Dictionary of YARP-encoded vocabs as keys and their values.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ inv()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::inv
+ (
+ const std::vector< double > &
+ xd ,
+
+
+
+
+ std::vector< double > &
+ q
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Perform inverse kinematics (using robot position as initial guess), but do not move.
+
Parameters
+
+ xd 6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ q Vector describing current position in joint space (meters or degrees).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ movj()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::movj
+ (
+ const std::vector< double > &
+ xd )
+
+
+
+
+
+override virtual
+
+
+
+
Perform inverse kinematics and move to desired position in joint space using absolute coordinates.
+
Parameters
+
+ xd 6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
See also relj (relative coordinates)
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ movl()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::movl
+ (
+ const std::vector< double > &
+ xd )
+
+
+
+
+
+override virtual
+
+
+
+
Move to end position along a line trajectory.
+
Parameters
+
+ xd 6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ movv()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::movv
+ (
+ const std::vector< double > &
+ xdotd )
+
+
+
+
+
+override virtual
+
+
+
+
Move along a line with constant velocity.
+
Parameters
+
+ xdotd 6-element vector describing desired velocity in cartesian space; first three elements denote translational velocity (meters/second), last three denote angular velocity (radians/second).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ pose()
+
+
+
+
+
+
+
+
+ void BasicCartesianControl::pose
+ (
+ const std::vector< double > &
+ x )
+
+
+
+
+
+override virtual
+
+
+
+
Move to desired position instantaneously, no further intermediate calculations are expected other than computing the inverse kinematics.
+
Parameters
+
+ x 6-element vector describing desired instantaneous pose in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ relj()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::relj
+ (
+ const std::vector< double > &
+ xd )
+
+
+
+
+
+override virtual
+
+
+
+
Perform inverse kinematics and move to desired position in joint space using relative coordinates.
+
Parameters
+
+ xd 6-element vector describing desired offset in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
See also movj (absolute coordinates)
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ setParameter()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::setParameter
+ (
+ int
+ vocab ,
+
+
+
+
+ double
+ value
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Ask the controller to store or update a parameter of 'double' type.
+
Parameters
+
+ vocab YARP-encoded vocab (parameter key).
+ value Parameter value encoded as a double.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ setParameters()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::setParameters
+ (
+ const std::map< int, double > &
+ params )
+
+
+
+
+
+override virtual
+
+
+
+
Ask the controller to store or update multiple parameters at once.
+
Parameters
+
+ params Dictionary of YARP-encoded vocabs as keys and their values.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ stat()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::stat
+ (
+ std::vector< double > &
+ x ,
+
+
+
+
+ int *
+ state = nullptr
,
+
+
+
+
+ double *
+ timestamp = nullptr
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Inform on control state, get robot position and perform forward kinematics.
+
Parameters
+
+ x 6-element vector describing current position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ state Identifier for a cartesian control vocab.
+ timestamp Remote encoder acquisition time.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ stopControl()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::stopControl
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ tool()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::tool
+ (
+ const std::vector< double > &
+ x )
+
+
+
+
+
+override virtual
+
+
+
+
Unload current tool if any and append new tool frame to the kinematic chain.
+
Parameters
+
+ x 6-element vector describing new tool tip with regard to current end-effector frame in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ twist()
+
+
+
+
+
+
+
+
+ void BasicCartesianControl::twist
+ (
+ const std::vector< double > &
+ xdot )
+
+
+
+
+
+override virtual
+
+
+
+
Move in instantaneous velocity increments.
+
Parameters
+
+ xdot 6-element vector describing velocity increments in cartesian space; first three elements denote translational velocity (meters/second), last three denote angular velocity (radians/second).
+
+
+
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ wait()
+
+
+
+
+
+
+
+
+ bool BasicCartesianControl::wait
+ (
+ double
+ timeout )
+
+
+
+
+
+override virtual
+
+
+
+
Block execution until the movement is completed, errors occur or timeout is reached.
+
Parameters
+
+ timeout Timeout in seconds, '0.0' means no timeout.
+
+
+
+
Returns true on success, false if errors occurred during the execution of the trajectory
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ wrench()
+
+
+
+
+
+
+
+
+ void BasicCartesianControl::wrench
+ (
+ const std::vector< double > &
+ w )
+
+
+
+
+
+override virtual
+
+
+
+
Make the TCP exert the desired force instantaneously.
+
Parameters
+
+ w 6-element vector describing desired force exerted by the TCP in cartesian space; first three elements denote linear force (Newton), last three denote torque (Newton*meters).
+
+
+
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
+
◆ fd
+
+
+
+
+
+
+
+
+ std::vector<double> roboticslab::BasicCartesianControl::fd
+
+
+
+
+private
+
+
+
+
FORC desired Cartesian force
+
+
+
+
+
◆ movementStartTime
+
+
+
+
+
+
+
+
+ double roboticslab::BasicCartesianControl::movementStartTime
+
+
+
+
+private
+
+
+
+
MOVL keep track of movement start time to know at what time of trajectory movement we are
+
+
+
+
+
◆ trajectories
+
+
+
+
+
+
+
+
+ std::vector<std::unique_ptr<KDL::Trajectory> > roboticslab::BasicCartesianControl::trajectories
+
+
+
+
+private
+
+
+
+
MOVL store Cartesian trajectory
+
+
+
+
+
◆ vmoStored
+
+
+
+
+
+
+
+
+ std::vector<double> roboticslab::BasicCartesianControl::vmoStored
+
+
+
+
+private
+
+
+
+
MOVJ store previous reference speeds
+
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp
+libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp
+libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp
+libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp
+libraries/YarpPlugins/BasicCartesianControl/PeriodicThreadImpl.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1BasicCartesianControl.png b/classroboticslab_1_1BasicCartesianControl.png
new file mode 100644
index 000000000..b3bdd51d5
Binary files /dev/null and b/classroboticslab_1_1BasicCartesianControl.png differ
diff --git a/classroboticslab_1_1BasicCartesianControl_1_1StateWatcher-members.html b/classroboticslab_1_1BasicCartesianControl_1_1StateWatcher-members.html
new file mode 100644
index 000000000..9a2f99538
--- /dev/null
+++ b/classroboticslab_1_1BasicCartesianControl_1_1StateWatcher-members.html
@@ -0,0 +1,92 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::BasicCartesianControl::StateWatcher , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1BasicCartesianControl_1_1StateWatcher.html b/classroboticslab_1_1BasicCartesianControl_1_1StateWatcher.html
new file mode 100644
index 000000000..2169db06c
--- /dev/null
+++ b/classroboticslab_1_1BasicCartesianControl_1_1StateWatcher.html
@@ -0,0 +1,109 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::BasicCartesianControl::StateWatcher Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+template<typename Fn >
+ StateWatcher (Fn &&fn)
+
+
+void suppress () const
+
+
+
+
+std::function< void()> handler
+
+
+
The documentation for this class was generated from the following file:
+
+
+
+
+
diff --git a/classroboticslab_1_1CartesianControlClient-members.html b/classroboticslab_1_1CartesianControlClient-members.html
new file mode 100644
index 000000000..ee000ce37
--- /dev/null
+++ b/classroboticslab_1_1CartesianControlClient-members.html
@@ -0,0 +1,121 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::CartesianControlClient , including all inherited members.
+
+ act (int command) overrideroboticslab::CartesianControlClient virtual
+ close () override (defined in roboticslab::CartesianControlClient )roboticslab::CartesianControlClient
+ commandPort (defined in roboticslab::CartesianControlClient )roboticslab::CartesianControlClient private
+ fkInPort (defined in roboticslab::CartesianControlClient )roboticslab::CartesianControlClient private
+ fkStreamResponder (defined in roboticslab::CartesianControlClient )roboticslab::CartesianControlClient private
+ fkStreamTimeoutSecs (defined in roboticslab::CartesianControlClient )roboticslab::CartesianControlClient private
+ forc (const std::vector< double > &fd) overrideroboticslab::CartesianControlClient virtual
+ gcmp () overrideroboticslab::CartesianControlClient virtual
+ getParameter (int vocab, double *value) overrideroboticslab::CartesianControlClient virtual
+ getParameters (std::map< int, double > ¶ms) overrideroboticslab::CartesianControlClient virtual
+ handleRpcConsumerCmd (int vocab, const std::vector< double > &in) (defined in roboticslab::CartesianControlClient )roboticslab::CartesianControlClient private
+ handleRpcFunctionCmd (int vocab, const std::vector< double > &in, std::vector< double > &out) (defined in roboticslab::CartesianControlClient )roboticslab::CartesianControlClient private
+ handleRpcRunnableCmd (int vocab) (defined in roboticslab::CartesianControlClient )roboticslab::CartesianControlClient private
+ handleStreamingBiConsumerCmd (int vocab, const std::vector< double > &in1, double in2) (defined in roboticslab::CartesianControlClient )roboticslab::CartesianControlClient private
+ handleStreamingConsumerCmd (int vocab, const std::vector< double > &in) (defined in roboticslab::CartesianControlClient )roboticslab::CartesianControlClient private
+ inv (const std::vector< double > &xd, std::vector< double > &q) overrideroboticslab::CartesianControlClient virtual
+ movi (const std::vector< double > &x) (defined in roboticslab::ICartesianControl )roboticslab::ICartesianControl inline virtual
+ movj (const std::vector< double > &xd) overrideroboticslab::CartesianControlClient virtual
+ movl (const std::vector< double > &xd) overrideroboticslab::CartesianControlClient virtual
+ movv (const std::vector< double > &xdotd) overrideroboticslab::CartesianControlClient virtual
+ open (yarp::os::Searchable &config) override (defined in roboticslab::CartesianControlClient )roboticslab::CartesianControlClient
+ pose (const std::vector< double > &x) overrideroboticslab::CartesianControlClient virtual
+ relj (const std::vector< double > &xd) overrideroboticslab::CartesianControlClient virtual
+ rpcClient (defined in roboticslab::CartesianControlClient )roboticslab::CartesianControlClient private
+ setParameter (int vocab, double value) overrideroboticslab::CartesianControlClient virtual
+ setParameters (const std::map< int, double > ¶ms) overrideroboticslab::CartesianControlClient virtual
+ stat (std::vector< double > &x, int *state=nullptr, double *timestamp=nullptr) overrideroboticslab::CartesianControlClient virtual
+ stopControl () overrideroboticslab::CartesianControlClient virtual
+ tool (const std::vector< double > &x) overrideroboticslab::CartesianControlClient virtual
+ twist (const std::vector< double > &xdot) overrideroboticslab::CartesianControlClient virtual
+ wait (double timeout) overrideroboticslab::CartesianControlClient virtual
+ wrench (const std::vector< double > &w) overrideroboticslab::CartesianControlClient virtual
+ ~ICartesianControl ()=defaultroboticslab::ICartesianControl virtual
+
+
+
+
+
diff --git a/classroboticslab_1_1CartesianControlClient.html b/classroboticslab_1_1CartesianControlClient.html
new file mode 100644
index 000000000..2a9ba130d
--- /dev/null
+++ b/classroboticslab_1_1CartesianControlClient.html
@@ -0,0 +1,941 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::CartesianControlClient Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
The CartesianControlClient class implements ICartesianControl client side.
+
+
+
#include <CartesianControlClient.hpp >
+
+
+
+
+
+
+
+
+
+
+bool stat (std::vector< double > &x, int *state=nullptr, double *timestamp=nullptr) override
+ Current state and position. More...
+
+bool inv (const std::vector< double > &xd, std::vector< double > &q) override
+ Inverse kinematics. More...
+
+bool movj (const std::vector< double > &xd) override
+ Move in joint space, absolute coordinates. More...
+
+bool relj (const std::vector< double > &xd) override
+ Move in joint space, relative coordinates. More...
+
+bool movl (const std::vector< double > &xd) override
+ Linear move to target position. More...
+
+bool movv (const std::vector< double > &xdotd) override
+ Linear move with given velocity. More...
+
+bool gcmp () override
+ Gravity compensation. More...
+
+bool forc (const std::vector< double > &fd) override
+ Force control. More...
+
+bool stopControl () override
+ Stop control. More...
+
+bool wait (double timeout) override
+ Wait until completion. More...
+
+bool tool (const std::vector< double > &x) override
+ Change tool. More...
+
+bool act (int command) override
+ Actuate tool. More...
+
+void pose (const std::vector< double > &x) override
+ Achieve pose. More...
+
+void twist (const std::vector< double > &xdot) override
+ Instantaneous velocity steps. More...
+
+void wrench (const std::vector< double > &w) override
+ Exert force. More...
+
+bool setParameter (int vocab, double value) override
+ Set a configuration parameter. More...
+
+bool getParameter (int vocab, double *value) override
+ Retrieve a configuration parameter. More...
+
+bool setParameters (const std::map< int, double > ¶ms) override
+ Set multiple configuration parameters. More...
+
+bool getParameters (std::map< int, double > ¶ms) override
+ Retrieve multiple configuration parameters. More...
+
+
+bool open (yarp::os::Searchable &config) override
+
+
+bool close () override
+
+
+
+virtual ~ICartesianControl ()=default
+ Destructor.
+
+
+virtual void movi (const std::vector< double > &x)
+
+
+
+
+bool handleRpcRunnableCmd (int vocab)
+
+
+bool handleRpcConsumerCmd (int vocab, const std::vector< double > &in)
+
+
+bool handleRpcFunctionCmd (int vocab, const std::vector< double > &in, std::vector< double > &out)
+
+
+void handleStreamingConsumerCmd (int vocab, const std::vector< double > &in)
+
+
+void handleStreamingBiConsumerCmd (int vocab, const std::vector< double > &in1, double in2)
+
+
+
+
+yarp::os::RpcClient rpcClient
+
+
+yarp::os::BufferedPort< yarp::os::Bottle > fkInPort
+
+
+yarp::os::BufferedPort< yarp::os::Bottle > commandPort
+
+
+FkStreamResponder fkStreamResponder
+
+
+double fkStreamTimeoutSecs
+
+
+
+
+
◆ act()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::act
+ (
+ int
+ command )
+
+
+
+
+
+override virtual
+
+
+
+
Send control command to actuate the robot's tool, if available.
+
Parameters
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ forc()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::forc
+ (
+ const std::vector< double > &
+ fd )
+
+
+
+
+
+override virtual
+
+
+
+
Apply desired forces in task space.
+
Parameters
+
+ fd 6-element vector describing desired force exerted by the TCP in cartesian space; first three elements denote linear force (Newton), last three denote torque (Newton*meters).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ gcmp()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::gcmp
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ getParameter()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::getParameter
+ (
+ int
+ vocab ,
+
+
+
+
+ double *
+ value
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Ask the controller to retrieve a parameter of 'double' type.
+
Parameters
+
+ vocab YARP-encoded vocab (parameter key).
+ value Parameter value encoded as a double.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ getParameters()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::getParameters
+ (
+ std::map< int, double > &
+ params )
+
+
+
+
+
+override virtual
+
+
+
+
Ask the controller to retrieve all available parameters at once.
+
Parameters
+
+ params Dictionary of YARP-encoded vocabs as keys and their values.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ inv()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::inv
+ (
+ const std::vector< double > &
+ xd ,
+
+
+
+
+ std::vector< double > &
+ q
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Perform inverse kinematics (using robot position as initial guess), but do not move.
+
Parameters
+
+ xd 6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ q Vector describing current position in joint space (meters or degrees).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ movj()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::movj
+ (
+ const std::vector< double > &
+ xd )
+
+
+
+
+
+override virtual
+
+
+
+
Perform inverse kinematics and move to desired position in joint space using absolute coordinates.
+
Parameters
+
+ xd 6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
See also relj (relative coordinates)
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ movl()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::movl
+ (
+ const std::vector< double > &
+ xd )
+
+
+
+
+
+override virtual
+
+
+
+
Move to end position along a line trajectory.
+
Parameters
+
+ xd 6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ movv()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::movv
+ (
+ const std::vector< double > &
+ xdotd )
+
+
+
+
+
+override virtual
+
+
+
+
Move along a line with constant velocity.
+
Parameters
+
+ xdotd 6-element vector describing desired velocity in cartesian space; first three elements denote translational velocity (meters/second), last three denote angular velocity (radians/second).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ pose()
+
+
+
+
+
+
+
+
+ void CartesianControlClient::pose
+ (
+ const std::vector< double > &
+ x )
+
+
+
+
+
+override virtual
+
+
+
+
Move to desired position instantaneously, no further intermediate calculations are expected other than computing the inverse kinematics.
+
Parameters
+
+ x 6-element vector describing desired instantaneous pose in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ relj()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::relj
+ (
+ const std::vector< double > &
+ xd )
+
+
+
+
+
+override virtual
+
+
+
+
Perform inverse kinematics and move to desired position in joint space using relative coordinates.
+
Parameters
+
+ xd 6-element vector describing desired offset in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
See also movj (absolute coordinates)
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ setParameter()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::setParameter
+ (
+ int
+ vocab ,
+
+
+
+
+ double
+ value
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Ask the controller to store or update a parameter of 'double' type.
+
Parameters
+
+ vocab YARP-encoded vocab (parameter key).
+ value Parameter value encoded as a double.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ setParameters()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::setParameters
+ (
+ const std::map< int, double > &
+ params )
+
+
+
+
+
+override virtual
+
+
+
+
Ask the controller to store or update multiple parameters at once.
+
Parameters
+
+ params Dictionary of YARP-encoded vocabs as keys and their values.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ stat()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::stat
+ (
+ std::vector< double > &
+ x ,
+
+
+
+
+ int *
+ state = nullptr
,
+
+
+
+
+ double *
+ timestamp = nullptr
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Inform on control state, get robot position and perform forward kinematics.
+
Parameters
+
+ x 6-element vector describing current position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ state Identifier for a cartesian control vocab.
+ timestamp Remote encoder acquisition time.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ stopControl()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::stopControl
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ tool()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::tool
+ (
+ const std::vector< double > &
+ x )
+
+
+
+
+
+override virtual
+
+
+
+
Unload current tool if any and append new tool frame to the kinematic chain.
+
Parameters
+
+ x 6-element vector describing new tool tip with regard to current end-effector frame in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ twist()
+
+
+
+
+
+
+
+
+ void CartesianControlClient::twist
+ (
+ const std::vector< double > &
+ xdot )
+
+
+
+
+
+override virtual
+
+
+
+
Move in instantaneous velocity increments.
+
Parameters
+
+ xdot 6-element vector describing velocity increments in cartesian space; first three elements denote translational velocity (meters/second), last three denote angular velocity (radians/second).
+
+
+
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ wait()
+
+
+
+
+
+
+
+
+ bool CartesianControlClient::wait
+ (
+ double
+ timeout )
+
+
+
+
+
+override virtual
+
+
+
+
Block execution until the movement is completed, errors occur or timeout is reached.
+
Parameters
+
+ timeout Timeout in seconds, '0.0' means no timeout.
+
+
+
+
Returns true on success, false if errors occurred during the execution of the trajectory
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
+
◆ wrench()
+
+
+
+
+
+
+
+
+ void CartesianControlClient::wrench
+ (
+ const std::vector< double > &
+ w )
+
+
+
+
+
+override virtual
+
+
+
+
Make the TCP exert the desired force instantaneously.
+
Parameters
+
+ w 6-element vector describing desired force exerted by the TCP in cartesian space; first three elements denote linear force (Newton), last three denote torque (Newton*meters).
+
+
+
+
+
Implements roboticslab::ICartesianControl .
+
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp
+libraries/YarpPlugins/CartesianControlClient/DeviceDriverImpl.cpp
+libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1CartesianControlClient.png b/classroboticslab_1_1CartesianControlClient.png
new file mode 100644
index 000000000..447db861a
Binary files /dev/null and b/classroboticslab_1_1CartesianControlClient.png differ
diff --git a/classroboticslab_1_1CartesianControlServer-members.html b/classroboticslab_1_1CartesianControlServer-members.html
new file mode 100644
index 000000000..66906d5e9
--- /dev/null
+++ b/classroboticslab_1_1CartesianControlServer-members.html
@@ -0,0 +1,102 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::CartesianControlServer , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1CartesianControlServer.html b/classroboticslab_1_1CartesianControlServer.html
new file mode 100644
index 000000000..f09224c34
--- /dev/null
+++ b/classroboticslab_1_1CartesianControlServer.html
@@ -0,0 +1,151 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::CartesianControlServer Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
The CartesianControlServer class implements ICartesianControl server side.
+
+
+
#include <CartesianControlServer.hpp >
+
+
+
+
+
+
+
+
+bool open (yarp::os::Searchable &config) override
+
+
+bool close () override
+
+
+void run () override
+
+
+
+
+yarp::dev::PolyDriver cartesianControlDevice
+
+
+yarp::os::RpcServer rpcServer
+
+
+yarp::os::RpcServer rpcTransformServer
+
+
+yarp::os::BufferedPort< yarp::os::Bottle > fkOutPort
+
+
+yarp::os::BufferedPort< yarp::os::Bottle > commandPort
+
+
+roboticslab::ICartesianControl * iCartesianControl {nullptr}
+
+
+RpcResponder * rpcResponder {nullptr}
+
+
+RpcResponder * rpcTransformResponder {nullptr}
+
+
+StreamResponder * streamResponder {nullptr}
+
+
+bool fkStreamEnabled
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp
+libraries/YarpPlugins/CartesianControlServer/DeviceDriverImpl.cpp
+libraries/YarpPlugins/CartesianControlServer/PeriodicThreadImpl.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1CartesianControlServer.png b/classroboticslab_1_1CartesianControlServer.png
new file mode 100644
index 000000000..3bf5876e0
Binary files /dev/null and b/classroboticslab_1_1CartesianControlServer.png differ
diff --git a/classroboticslab_1_1CentroidTransform-members.html b/classroboticslab_1_1CentroidTransform-members.html
new file mode 100644
index 000000000..df2343f14
--- /dev/null
+++ b/classroboticslab_1_1CentroidTransform-members.html
@@ -0,0 +1,99 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::CentroidTransform , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1CentroidTransform.html b/classroboticslab_1_1CentroidTransform.html
new file mode 100644
index 000000000..7b8c733c0
--- /dev/null
+++ b/classroboticslab_1_1CentroidTransform.html
@@ -0,0 +1,143 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::CentroidTransform Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
#include <CentroidTransform.hpp >
+
+
+
+StreamingDevice * streamingDevice
+
+
+double permanenceTime
+
+
+yarp::os::Bottle lastBottle
+
+
+yarp::os::Stamp lastAcquisition
+
+
+KDL::Rotation rot_tcp_camera
+
+
+
+
The documentation for this class was generated from the following files:
+programs/streamingDeviceController/CentroidTransform.hpp
+programs/streamingDeviceController/CentroidTransform.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1ChainFkSolverPos__ST-members.html b/classroboticslab_1_1ChainFkSolverPos__ST-members.html
new file mode 100644
index 000000000..80efd9937
--- /dev/null
+++ b/classroboticslab_1_1ChainFkSolverPos__ST-members.html
@@ -0,0 +1,98 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ChainFkSolverPos_ST , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1ChainFkSolverPos__ST.html b/classroboticslab_1_1ChainFkSolverPos__ST.html
new file mode 100644
index 000000000..2be0f1c47
--- /dev/null
+++ b/classroboticslab_1_1ChainFkSolverPos__ST.html
@@ -0,0 +1,353 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ChainFkSolverPos_ST Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
FK solver using Screw Theory.
+ More...
+
+
#include <ChainFkSolverPos_ST.hpp >
+
+
+
+
+
+
+
+int JntToCart (const KDL::JntArray &q_in, KDL::Frame &p_out, int segmentNr=-1) override
+ Perform FK on the selected segment. More...
+
+int JntToCart (const KDL::JntArray &q_in, std::vector< KDL::Frame > &p_out, int segmentNr=-1) override
+ Perform FK on the selected segments (unsupported) More...
+
+void updateInternalDataStructures () override
+ Update the internal data structures. More...
+
+const char * strError (const int error) const override
+ Return a description of the last error. More...
+
+
+
+
+ ChainFkSolverPos_ST (const KDL::Chain &chain)
+
+
+
+
Implementation of a forward position kinematics algorithm. This is a thin wrapper around PoeExpression . Methods that retrieve resulting frames for intermediate links are not supported.
+
+
+
◆ create()
+
+
+
+
+
+
+
+
+ KDL::ChainFkSolverPos * ChainFkSolverPos_ST::create
+ (
+ const KDL::Chain &
+ chain )
+
+
+
+
+
+static
+
+
+
+
Parameters
+
+ chain Input kinematic chain.
+
+
+
+
Returns Solver instance.
+
+
+
+
+
◆ JntToCart() [1/2]
+
+
+
+
+
+
+
+
+ int ChainFkSolverPos_ST::JntToCart
+ (
+ const KDL::JntArray &
+ q_in ,
+
+
+
+
+ KDL::Frame &
+ p_out ,
+
+
+
+
+ int
+ segmentNr = -1
+
+
+
+ )
+
+
+
+
+
+override
+
+
+
+
Parameters
+
+ q_in Input joint coordinates.
+ p_out Reference to output cartesian pose.
+ segmentNr Desired segment frame (unsupported).
+
+
+
+
Returns Return code, < 0 if something went wrong.
+
+
+
+
+
◆ JntToCart() [2/2]
+
+
+
+
+
+
+
+
+ int ChainFkSolverPos_ST::JntToCart
+ (
+ const KDL::JntArray &
+ q_in ,
+
+
+
+
+ std::vector< KDL::Frame > &
+ p_out ,
+
+
+
+
+ int
+ segmentNr = -1
+
+
+
+ )
+
+
+
+
+
+override
+
+
+
+
Parameters
+
+ q_in Input joint coordinates.
+ p_out Reference to a vector of output cartesian poses for all segments.
+ segmentNr Last selected segment frame.
+
+
+
+
Returns Return code, < 0 if something went wrong.
+
Warning Unsupported, will return E_OPERATION_NOT_SUPPORTED .
+
+
+
+
+
◆ strError()
+
+
+
+
+
+
+
+
+ const char * ChainFkSolverPos_ST::strError
+ (
+ const int
+ error )
+ const
+
+
+
+
+override
+
+
+
+
Parameters
+
+
+
+
Returns If error
is known then a description of error
, otherwise "UNKNOWN ERROR".
+
+
+
+
+
◆ updateInternalDataStructures()
+
+
+
+
+
+
+
+
+ void ChainFkSolverPos_ST::updateInternalDataStructures
+ (
+ )
+
+
+
+
+
+override
+
+
+
+
Update the internal data structures. This is required if the number of segments or number of joints of a chain has changed. This provides a single point of contact for solver memory allocations.
+
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/KdlSolver/ChainFkSolverPos_ST.hpp
+libraries/YarpPlugins/KdlSolver/ChainFkSolverPos_ST.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1ChainFkSolverPos__ST.png b/classroboticslab_1_1ChainFkSolverPos__ST.png
new file mode 100644
index 000000000..133627f34
Binary files /dev/null and b/classroboticslab_1_1ChainFkSolverPos__ST.png differ
diff --git a/classroboticslab_1_1ChainIkSolverPos__ID-members.html b/classroboticslab_1_1ChainIkSolverPos__ID-members.html
new file mode 100644
index 000000000..b9513b6f2
--- /dev/null
+++ b/classroboticslab_1_1ChainIkSolverPos__ID-members.html
@@ -0,0 +1,102 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ChainIkSolverPos_ID , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1ChainIkSolverPos__ID.html b/classroboticslab_1_1ChainIkSolverPos__ID.html
new file mode 100644
index 000000000..26bf77731
--- /dev/null
+++ b/classroboticslab_1_1ChainIkSolverPos__ID.html
@@ -0,0 +1,327 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ChainIkSolverPos_ID Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
IK solver using infinitesimal displacement twists.
+ More...
+
+
#include <ChainIkSolverPos_ID.hpp >
+
+
+
+
+
+
+
+ ChainIkSolverPos_ID (const KDL::Chain &chain, const KDL::JntArray &q_min, const KDL::JntArray &q_max, KDL::ChainFkSolverPos &fksolver)
+ Constructor. More...
+
+int CartToJnt (const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out) override
+ Calculate inverse position kinematics. More...
+
+void updateInternalDataStructures () override
+ Update the internal data structures. More...
+
+const char * strError (const int error) const override
+ Return a description of the last error. More...
+
+
+
+
+static const int E_FKSOLVERPOS_FAILED = -100
+ Return code, internal FK position solver failed.
+
+
+static const int E_JACSOLVER_FAILED = -101
+ Return code, internal Jacobian solver failed.
+
+
+
+
+KDL::JntArray computeDiffInvKin (const KDL::Twist &delta_twist)
+
+
+
+
+const KDL::Chain & chain
+
+
+unsigned int nj
+
+
+KDL::JntArray qMin
+
+
+KDL::JntArray qMax
+
+
+KDL::ChainFkSolverPos & fkSolverPos
+
+
+KDL::ChainJntToJacSolver jacSolver
+
+
+KDL::Jacobian jacobian
+
+
+
+
Re-implementation of KDL::ChainIkSolverPos_NR_JL in which only one iteration step is performed. Aimed to provide a quick means of obtaining IK whenever the displacements are small enough.
+
+
+
◆ ChainIkSolverPos_ID()
+
+
+
+
+
+ ChainIkSolverPos_ID::ChainIkSolverPos_ID
+ (
+ const KDL::Chain &
+ chain ,
+
+
+
+
+ const KDL::JntArray &
+ q_min ,
+
+
+
+
+ const KDL::JntArray &
+ q_max ,
+
+
+
+
+ KDL::ChainFkSolverPos &
+ fksolver
+
+
+
+ )
+
+
+
+
+
Parameters
+
+ chain The chain to calculate the inverse position for.
+ q_min The minimum joint positions.
+ q_max The maximum joint positions.
+ fksolver A forward position kinematics solver.
+ iksolver An inverse velocity kinematics solver.
+
+
+
+
+
+
+
+
+
◆ CartToJnt()
+
+
+
+
+
+
+
+
+ int ChainIkSolverPos_ID::CartToJnt
+ (
+ const KDL::JntArray &
+ q_init ,
+
+
+
+
+ const KDL::Frame &
+ p_in ,
+
+
+
+
+ KDL::JntArray &
+ q_out
+
+
+
+ )
+
+
+
+
+
+override
+
+
+
+
Parameters
+
+ q_init Initial guess of the joint coordinates (unused).
+ p_in Input cartesian coordinates.
+ q_out Output joint coordinates.
+
+
+
+
Returns Return code, E_SOLUTION_NOT_FOUND if there is no solution or E_NOT_REACHABLE if at least one of them is out of reach.
+
+
+
+
+
◆ strError()
+
+
+
+
+
+
+
+
+ const char * ChainIkSolverPos_ID::strError
+ (
+ const int
+ error )
+ const
+
+
+
+
+override
+
+
+
+
Parameters
+
+
+
+
Returns If error
is known then a description of error
, otherwise "UNKNOWN ERROR".
+
+
+
+
+
◆ updateInternalDataStructures()
+
+
+
+
+
+
+
+
+ void ChainIkSolverPos_ID::updateInternalDataStructures
+ (
+ )
+
+
+
+
+
+override
+
+
+
+
Update the internal data structures. This is required if the number of segments or number of joints of a chain has changed. This provides a single point of contact for solver memory allocations.
+
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/KdlSolver/ChainIkSolverPos_ID.hpp
+libraries/YarpPlugins/KdlSolver/ChainIkSolverPos_ID.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1ChainIkSolverPos__ID.png b/classroboticslab_1_1ChainIkSolverPos__ID.png
new file mode 100644
index 000000000..ec79b88f9
Binary files /dev/null and b/classroboticslab_1_1ChainIkSolverPos__ID.png differ
diff --git a/classroboticslab_1_1ChainIkSolverPos__ST-members.html b/classroboticslab_1_1ChainIkSolverPos__ST-members.html
new file mode 100644
index 000000000..9a4b1a235
--- /dev/null
+++ b/classroboticslab_1_1ChainIkSolverPos__ST-members.html
@@ -0,0 +1,100 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ChainIkSolverPos_ST , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1ChainIkSolverPos__ST.html b/classroboticslab_1_1ChainIkSolverPos__ST.html
new file mode 100644
index 000000000..5915ad903
--- /dev/null
+++ b/classroboticslab_1_1ChainIkSolverPos__ST.html
@@ -0,0 +1,320 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ChainIkSolverPos_ST Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
IK solver using Screw Theory.
+ More...
+
+
#include <ChainIkSolverPos_ST.hpp >
+
+
+
+
+
+
+
+
+static const int E_SOLUTION_NOT_FOUND = -100
+ Return code, IK solution not found.
+
+
+static const int E_OUT_OF_LIMITS = -101
+ Return code, target pose out of robot limits.
+
+
+static const int E_NOT_REACHABLE = 100
+ Return code, solution out of reach.
+
+
+
+
Implementation of an inverse position kinematics algorithm. This is a thin wrapper around ScrewTheoryIkProblem . Non-exhaustive tests on TEO's (UC3M) right arm kinematic chain reveal that this is 5-10 faster than a numeric Newton-Raphson solver as provided by KDL (e.g. KDL::ChainIkSolverPos_NR_JL).
+
+
+
◆ CartToJnt()
+
+
+
+
+
+
+
+
+ int ChainIkSolverPos_ST::CartToJnt
+ (
+ const KDL::JntArray &
+ q_init ,
+
+
+
+
+ const KDL::Frame &
+ p_in ,
+
+
+
+
+ KDL::JntArray &
+ q_out
+
+
+
+ )
+
+
+
+
+
+override
+
+
+
+
Parameters
+
+ q_init Initial guess of the joint coordinates (unused).
+ p_in Input cartesian coordinates.
+ q_out Output joint coordinates.
+
+
+
+
Returns Return code, E_SOLUTION_NOT_FOUND if there is no solution or E_NOT_REACHABLE if at least one of them is out of reach.
+
+
+
+
+
◆ create()
+
+
+
+
+
+
+
+
+ KDL::ChainIkSolverPos * ChainIkSolverPos_ST::create
+ (
+ const KDL::Chain &
+ chain ,
+
+
+
+
+ const ConfigurationSelectorFactory &
+ configFactory
+
+
+
+ )
+
+
+
+
+
+static
+
+
+
+
Parameters
+
+ chain Input kinematic chain.
+ configFactory Instance of an abstract factory class that instantiates a ConfigurationSelector .
+
+
+
+
Returns Solver instance or null if no solution was found.
+
+
+
+
+
◆ strError()
+
+
+
+
+
+
+
+
+ const char * ChainIkSolverPos_ST::strError
+ (
+ const int
+ error )
+ const
+
+
+
+
+override
+
+
+
+
Parameters
+
+
+
+
Returns If error
is known then a description of error
, otherwise "UNKNOWN ERROR".
+
+
+
+
+
◆ updateInternalDataStructures()
+
+
+
+
+
+
+
+
+ void ChainIkSolverPos_ST::updateInternalDataStructures
+ (
+ )
+
+
+
+
+
+override
+
+
+
+
Update the internal data structures. This is required if the number of segments or number of joints of a chain has changed. This provides a single point of contact for solver memory allocations.
+
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/KdlSolver/ChainIkSolverPos_ST.hpp
+libraries/YarpPlugins/KdlSolver/ChainIkSolverPos_ST.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1ChainIkSolverPos__ST.png b/classroboticslab_1_1ChainIkSolverPos__ST.png
new file mode 100644
index 000000000..2a56c99fd
Binary files /dev/null and b/classroboticslab_1_1ChainIkSolverPos__ST.png differ
diff --git a/classroboticslab_1_1ConfigurationSelector-members.html b/classroboticslab_1_1ConfigurationSelector-members.html
new file mode 100644
index 000000000..8137b72bf
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelector-members.html
@@ -0,0 +1,97 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ConfigurationSelector , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1ConfigurationSelector.html b/classroboticslab_1_1ConfigurationSelector.html
new file mode 100644
index 000000000..17baf99e7
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelector.html
@@ -0,0 +1,295 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ConfigurationSelector Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Abstract base class for a robot configuration strategy selector.
+
+
+
#include <ConfigurationSelector.hpp >
+
+
+
+
+
+
+
+
+
+
+
+
+
◆ ConfigurationSelector()
+
+
+
+
+
+
+
+
+ roboticslab::ConfigurationSelector::ConfigurationSelector
+ (
+ const KDL::JntArray &
+ qMin ,
+
+
+
+
+ const KDL::JntArray &
+ qMax
+
+
+
+ )
+
+
+
+
+
+inline
+
+
+
+
Parameters
+
+ qMin Joint array of minimum joint limits.
+ qMax Joint array of maximum joint limits.
+
+
+
+
+
+
+
+
+
◆ configure()
+
+
+
+
+
+
+
+
+ bool ConfigurationSelector::configure
+ (
+ const std::vector< KDL::JntArray > &
+ solutions )
+
+
+
+
+
+virtual
+
+
+
+
Parameters
+
+ solutions Vector of joint arrays that represent all available (valid or not) robot joint poses.
+
+
+
+
Returns True/false on success/failure.
+
+
+
+
+
◆ findOptimalConfiguration()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ConfigurationSelector::findOptimalConfiguration
+ (
+ const KDL::JntArray &
+ qGuess )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ retrievePose()
+
+
+
+
+
+
+
+
+ virtual void roboticslab::ConfigurationSelector::retrievePose
+ (
+ KDL::JntArray &
+ q )
+ const
+
+
+
+
+inline virtual
+
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1ConfigurationSelector.png b/classroboticslab_1_1ConfigurationSelector.png
new file mode 100644
index 000000000..b4665c964
Binary files /dev/null and b/classroboticslab_1_1ConfigurationSelector.png differ
diff --git a/classroboticslab_1_1ConfigurationSelectorFactory-members.html b/classroboticslab_1_1ConfigurationSelectorFactory-members.html
new file mode 100644
index 000000000..e68ea0895
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelectorFactory-members.html
@@ -0,0 +1,93 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ConfigurationSelectorFactory , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1ConfigurationSelectorFactory.html b/classroboticslab_1_1ConfigurationSelectorFactory.html
new file mode 100644
index 000000000..7422133c8
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelectorFactory.html
@@ -0,0 +1,205 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ConfigurationSelectorFactory Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Base factory class for ConfigurationSelector .
+ More...
+
+
#include <ConfigurationSelector.hpp >
+
+
+
+
+
+
+
+
+
+
+
+
+KDL::JntArray _qMin
+
+
+KDL::JntArray _qMax
+
+
+
+
Acts as the base class in the abstract factory pattern , encapsulates individual factories.
+
+
+
◆ ConfigurationSelectorFactory()
+
+
+
+
+
+
+
+
+ roboticslab::ConfigurationSelectorFactory::ConfigurationSelectorFactory
+ (
+ const KDL::JntArray &
+ qMin ,
+
+
+
+
+ const KDL::JntArray &
+ qMax
+
+
+
+ )
+
+
+
+
+
+inline protected
+
+
+
+
Parameters
+
+ qMin Joint array of minimum joint limits.
+ qMax Joint array of maximum joint limits.
+
+
+
+
+
+
+
+
+
◆ create()
+
+
+
+
+
+
+
+
+
+pure virtual
+
+
+
+
+
The documentation for this class was generated from the following file:
+
+
+
+
+
diff --git a/classroboticslab_1_1ConfigurationSelectorFactory.png b/classroboticslab_1_1ConfigurationSelectorFactory.png
new file mode 100644
index 000000000..0efb8245c
Binary files /dev/null and b/classroboticslab_1_1ConfigurationSelectorFactory.png differ
diff --git a/classroboticslab_1_1ConfigurationSelectorHumanoidGait-members.html b/classroboticslab_1_1ConfigurationSelectorHumanoidGait-members.html
new file mode 100644
index 000000000..db48d56a2
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelectorHumanoidGait-members.html
@@ -0,0 +1,103 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ConfigurationSelectorHumanoidGait , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1ConfigurationSelectorHumanoidGait.html b/classroboticslab_1_1ConfigurationSelectorHumanoidGait.html
new file mode 100644
index 000000000..2b33281ac
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelectorHumanoidGait.html
@@ -0,0 +1,254 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ConfigurationSelectorHumanoidGait Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
IK solver configuration strategy selector based on human walking.
+ More...
+
+
#include <ConfigurationSelector.hpp >
+
+
+
+
+
+
+
+
+
+
+
+
+bool applyConstraints (const Configuration &config)
+ Determines whether the configuration is valid according to this selector's premises.
+
+
+
+
+
+std::vector< double > getDiffs (const KDL::JntArray &qGuess, const Configuration &config)
+ Obtains vector of differences between current and desired joint values.
+
+
+
+int lastValid
+
+
+
+KDL::JntArray _qMin
+
+
+KDL::JntArray _qMax
+
+
+Configuration optimalConfig
+
+
+std::vector< Configuration > configs
+
+
+
+static const int INVALID_CONFIG = -1
+
+
+
+
Intended for use on lower-body limbs, specifically UC3M TEO's 6-DOF legs. Discards non-human-like configurations, such as the inverted knee pose.
+
+
+
◆ ConfigurationSelectorHumanoidGait()
+
+
+
+
+
+
+
+
+ roboticslab::ConfigurationSelectorHumanoidGait::ConfigurationSelectorHumanoidGait
+ (
+ const KDL::JntArray &
+ qMin ,
+
+
+
+
+ const KDL::JntArray &
+ qMax
+
+
+
+ )
+
+
+
+
+
+inline
+
+
+
+
Parameters
+
+ qMin Joint array of minimum joint limits.
+ qMax Joint array of maximum joint limits.
+
+
+
+
+
+
+
+
+
◆ findOptimalConfiguration()
+
+
+
+
+
+
+
+
+ bool ConfigurationSelectorHumanoidGait::findOptimalConfiguration
+ (
+ const KDL::JntArray &
+ qGuess )
+
+
+
+
+
+override virtual
+
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1ConfigurationSelectorHumanoidGait.png b/classroboticslab_1_1ConfigurationSelectorHumanoidGait.png
new file mode 100644
index 000000000..0616357a6
Binary files /dev/null and b/classroboticslab_1_1ConfigurationSelectorHumanoidGait.png differ
diff --git a/classroboticslab_1_1ConfigurationSelectorHumanoidGaitFactory-members.html b/classroboticslab_1_1ConfigurationSelectorHumanoidGaitFactory-members.html
new file mode 100644
index 000000000..9fdc62fbb
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelectorHumanoidGaitFactory-members.html
@@ -0,0 +1,94 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ConfigurationSelectorHumanoidGaitFactory , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1ConfigurationSelectorHumanoidGaitFactory.html b/classroboticslab_1_1ConfigurationSelectorHumanoidGaitFactory.html
new file mode 100644
index 000000000..1cccbca20
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelectorHumanoidGaitFactory.html
@@ -0,0 +1,204 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ConfigurationSelectorHumanoidGaitFactory Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Implementation factory class for ConfigurationSelectorHumanoidGait .
+ More...
+
+
#include <ConfigurationSelector.hpp >
+
+
+
+
+
+
+
+
+
+
+
+
+
◆ ConfigurationSelectorHumanoidGaitFactory()
+
+
+
+
+
+
+
+
+ roboticslab::ConfigurationSelectorHumanoidGaitFactory::ConfigurationSelectorHumanoidGaitFactory
+ (
+ const KDL::JntArray &
+ qMin ,
+
+
+
+
+ const KDL::JntArray &
+ qMax
+
+
+
+ )
+
+
+
+
+
+inline
+
+
+
+
Parameters
+
+ qMin Joint array of minimum joint limits.
+ qMax Joint array of maximum joint limits.
+
+
+
+
+
+
+
+
+
◆ create()
+
+
+
+
+
+
+
+
+ ConfigurationSelector * roboticslab::ConfigurationSelectorHumanoidGaitFactory::create
+ (
+ )
+ const
+
+
+
+
+inline override virtual
+
+
+
+
+
The documentation for this class was generated from the following file:
+
+
+
+
+
diff --git a/classroboticslab_1_1ConfigurationSelectorHumanoidGaitFactory.png b/classroboticslab_1_1ConfigurationSelectorHumanoidGaitFactory.png
new file mode 100644
index 000000000..afa98aa70
Binary files /dev/null and b/classroboticslab_1_1ConfigurationSelectorHumanoidGaitFactory.png differ
diff --git a/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacement-members.html b/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacement-members.html
new file mode 100644
index 000000000..b8435479e
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacement-members.html
@@ -0,0 +1,101 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ConfigurationSelectorLeastOverallAngularDisplacement , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacement.html b/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacement.html
new file mode 100644
index 000000000..9515a2194
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacement.html
@@ -0,0 +1,250 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ConfigurationSelectorLeastOverallAngularDisplacement Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
IK solver configuration strategy selector based on the overall displacement of all joints.
+ More...
+
+
#include <ConfigurationSelector.hpp >
+
+
+
+
+
+
+
+
+
+
+
+
+std::vector< double > getDiffs (const KDL::JntArray &qGuess, const Configuration &config)
+ Obtains vector of differences between current and desired joint values.
+
+
+
+
+int lastValid
+
+
+
+KDL::JntArray _qMin
+
+
+KDL::JntArray _qMax
+
+
+Configuration optimalConfig
+
+
+std::vector< Configuration > configs
+
+
+
+
+static const int INVALID_CONFIG = -1
+
+
+
+
Selects the configuration that entails the lowest sum of displacements across all joints. Works best for all revolute/all prismatic chain types. If attainable, it retains the previous configuration after the first successful choice and discards all other configs for the rest of the instance's lifetime.
+
+
+
◆ ConfigurationSelectorLeastOverallAngularDisplacement()
+
+
+
+
+
+
+
+
+ roboticslab::ConfigurationSelectorLeastOverallAngularDisplacement::ConfigurationSelectorLeastOverallAngularDisplacement
+ (
+ const KDL::JntArray &
+ qMin ,
+
+
+
+
+ const KDL::JntArray &
+ qMax
+
+
+
+ )
+
+
+
+
+
+inline
+
+
+
+
Parameters
+
+ qMin Joint array of minimum joint limits.
+ qMax Joint array of maximum joint limits.
+
+
+
+
+
+
+
+
+
◆ findOptimalConfiguration()
+
+
+
+
+
+
+
+
+ bool ConfigurationSelectorLeastOverallAngularDisplacement::findOptimalConfiguration
+ (
+ const KDL::JntArray &
+ qGuess )
+
+
+
+
+
+override virtual
+
+
+
+
+
The documentation for this class was generated from the following files:
+libraries/ScrewTheoryLib/ConfigurationSelector.hpp
+libraries/ScrewTheoryLib/ConfigurationSelectorLeastOverallAngularDisplacement.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacement.png b/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacement.png
new file mode 100644
index 000000000..5bb39e1a5
Binary files /dev/null and b/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacement.png differ
diff --git a/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacementFactory-members.html b/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacementFactory-members.html
new file mode 100644
index 000000000..f508f0d69
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacementFactory-members.html
@@ -0,0 +1,94 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ConfigurationSelectorLeastOverallAngularDisplacementFactory , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacementFactory.html b/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacementFactory.html
new file mode 100644
index 000000000..d519f6fe1
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacementFactory.html
@@ -0,0 +1,204 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ConfigurationSelectorLeastOverallAngularDisplacementFactory Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Implementation factory class for ConfigurationSelectorLeastOverallAngularDisplacement .
+ More...
+
+
#include <ConfigurationSelector.hpp >
+
+
+
+
+
+
+
+
+
+
+
+
+
◆ ConfigurationSelectorLeastOverallAngularDisplacementFactory()
+
+
+
+
+
+
+
+
+ roboticslab::ConfigurationSelectorLeastOverallAngularDisplacementFactory::ConfigurationSelectorLeastOverallAngularDisplacementFactory
+ (
+ const KDL::JntArray &
+ qMin ,
+
+
+
+
+ const KDL::JntArray &
+ qMax
+
+
+
+ )
+
+
+
+
+
+inline
+
+
+
+
Parameters
+
+ qMin Joint array of minimum joint limits.
+ qMax Joint array of maximum joint limits.
+
+
+
+
+
+
+
+
+
◆ create()
+
+
+
+
+
+
+
+
+ ConfigurationSelector * roboticslab::ConfigurationSelectorLeastOverallAngularDisplacementFactory::create
+ (
+ )
+ const
+
+
+
+
+inline override virtual
+
+
+
+
+
The documentation for this class was generated from the following file:
+
+
+
+
+
diff --git a/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacementFactory.png b/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacementFactory.png
new file mode 100644
index 000000000..6ffebc926
Binary files /dev/null and b/classroboticslab_1_1ConfigurationSelectorLeastOverallAngularDisplacementFactory.png differ
diff --git a/classroboticslab_1_1ConfigurationSelector_1_1Configuration-members.html b/classroboticslab_1_1ConfigurationSelector_1_1Configuration-members.html
new file mode 100644
index 000000000..50e9e1f47
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelector_1_1Configuration-members.html
@@ -0,0 +1,96 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ConfigurationSelector::Configuration , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1ConfigurationSelector_1_1Configuration.html b/classroboticslab_1_1ConfigurationSelector_1_1Configuration.html
new file mode 100644
index 000000000..8c9da80b9
--- /dev/null
+++ b/classroboticslab_1_1ConfigurationSelector_1_1Configuration.html
@@ -0,0 +1,135 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ConfigurationSelector::Configuration Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Helper class to store a specific robot configuration.
+
+
+
#include <ConfigurationSelector.hpp >
+
+
+
+ Configuration ()
+ Constructor.
+
+
+void store (const KDL::JntArray *q)
+ Initializes joint values.
+
+
+void validate (const KDL::JntArray &qMin, const KDL::JntArray &qMax)
+ Checks reachability against provided joint limits.
+
+
+const KDL::JntArray * retrievePose () const
+ Retrieves stored joint values.
+
+
+bool isValid () const
+ Whether this configuration is attainable or not.
+
+
+void invalidate ()
+ Mark this configuration as invalid.
+
+
+
+
+const KDL::JntArray * q
+
+
+bool valid
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1FkStreamResponder-members.html b/classroboticslab_1_1FkStreamResponder-members.html
new file mode 100644
index 000000000..bd74db0ab
--- /dev/null
+++ b/classroboticslab_1_1FkStreamResponder-members.html
@@ -0,0 +1,96 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::FkStreamResponder , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1FkStreamResponder.html b/classroboticslab_1_1FkStreamResponder.html
new file mode 100644
index 000000000..529462c29
--- /dev/null
+++ b/classroboticslab_1_1FkStreamResponder.html
@@ -0,0 +1,132 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::FkStreamResponder Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Responds to streaming FK messages.
+
+
+
#include <CartesianControlClient.hpp >
+
+
+
+
+
+
+
+
+void onRead (yarp::os::Bottle &b) override
+
+
+bool getLastStatData (std::vector< double > &x, int *state, double *timestamp, double timeout)
+
+
+
+
+double localArrivalTime
+
+
+int state
+
+
+double timestamp
+
+
+std::vector< double > x
+
+
+std::mutex mtx
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp
+libraries/YarpPlugins/CartesianControlClient/FkStreamResponder.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1FkStreamResponder.png b/classroboticslab_1_1FkStreamResponder.png
new file mode 100644
index 000000000..077ba5078
Binary files /dev/null and b/classroboticslab_1_1FkStreamResponder.png differ
diff --git a/classroboticslab_1_1FtCompensation-members.html b/classroboticslab_1_1FtCompensation-members.html
new file mode 100644
index 000000000..c47c39e67
--- /dev/null
+++ b/classroboticslab_1_1FtCompensation-members.html
@@ -0,0 +1,123 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::FtCompensation , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1FtCompensation.html b/classroboticslab_1_1FtCompensation.html
new file mode 100644
index 000000000..f3d09e44d
--- /dev/null
+++ b/classroboticslab_1_1FtCompensation.html
@@ -0,0 +1,222 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::FtCompensation Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Produces motion in the direction of an externally applied force measured by a force-torque sensor (pretty much mimicking a classical gravity compensation app).
+
+
+
#include <FtCompensation.hpp >
+
+
+
+
+
+
+
+
+bool configure (yarp::os::ResourceFinder &rf) override
+
+
+bool updateModule () override
+
+
+bool interruptModule () override
+
+
+double getPeriod () override
+
+
+bool close () override
+
+
+
+
+void run () override
+
+
+
+
+using cartesian_cmd = void(ICartesianControl::*)(const std::vector< double > &)
+
+
+
+
+bool readSensor (KDL::Wrench &wrench) const
+
+
+bool compensateTool (KDL::Wrench &wrench) const
+
+
+bool applyImpedance (KDL::Wrench &wrench)
+
+
+
+
+yarp::dev::PolyDriver cartesianDevice
+
+
+ICartesianControl * iCartesianControl
+
+
+int sensorIndex
+
+
+yarp::dev::PolyDriver sensorDevice
+
+
+yarp::dev::ISixAxisForceTorqueSensors * sensor
+
+
+KDL::Rotation R_N_sensor
+
+
+KDL::Vector toolCoM_N
+
+
+KDL::Wrench toolWeight_0
+
+
+KDL::Wrench initialOffset
+
+
+KDL::Frame initialPose
+
+
+KDL::Frame previousPose
+
+
+cartesian_cmd command
+
+
+bool dryRun
+
+
+bool usingTool
+
+
+bool enableImpedance
+
+
+double linGain
+
+
+double rotGain
+
+
+double linStiffness
+
+
+double rotStiffness
+
+
+double linDamping
+
+
+double rotDamping
+
+
+double forceDeadband
+
+
+double torqueDeadband
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1FtCompensation.png b/classroboticslab_1_1FtCompensation.png
new file mode 100644
index 000000000..ff4f9fb05
Binary files /dev/null and b/classroboticslab_1_1FtCompensation.png differ
diff --git a/classroboticslab_1_1GrabberResponder-members.html b/classroboticslab_1_1GrabberResponder-members.html
new file mode 100644
index 000000000..49f385abe
--- /dev/null
+++ b/classroboticslab_1_1GrabberResponder-members.html
@@ -0,0 +1,95 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::GrabberResponder , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1GrabberResponder.html b/classroboticslab_1_1GrabberResponder.html
new file mode 100644
index 000000000..9ecc2d314
--- /dev/null
+++ b/classroboticslab_1_1GrabberResponder.html
@@ -0,0 +1,129 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::GrabberResponder Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Callback class for dealing with incoming grabber data streams.
+
+
+
#include <GrabberResponder.hpp >
+
+
+
+
+
+
+
+
+void onRead (yarp::os::Bottle &b) override
+
+
+void setICartesianControlDriver (roboticslab::ICartesianControl *iCartesianControl)
+
+
+void setNoApproachSetting (bool noApproach)
+
+
+
The documentation for this class was generated from the following files:
+programs/haarDetectionController/GrabberResponder.hpp
+programs/haarDetectionController/GrabberResponder.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1GrabberResponder.png b/classroboticslab_1_1GrabberResponder.png
new file mode 100644
index 000000000..a1952a7f7
Binary files /dev/null and b/classroboticslab_1_1GrabberResponder.png differ
diff --git a/classroboticslab_1_1HaarDetectionController-members.html b/classroboticslab_1_1HaarDetectionController-members.html
new file mode 100644
index 000000000..7e51a47ec
--- /dev/null
+++ b/classroboticslab_1_1HaarDetectionController-members.html
@@ -0,0 +1,101 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::HaarDetectionController , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1HaarDetectionController.html b/classroboticslab_1_1HaarDetectionController.html
new file mode 100644
index 000000000..46c3d771f
--- /dev/null
+++ b/classroboticslab_1_1HaarDetectionController.html
@@ -0,0 +1,147 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::HaarDetectionController Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Create seek-and-follow trajectories based on Haar detection algorithms.
+
+
+
#include <HaarDetectionController.hpp >
+
+
+
+
+
+
+
+
+bool configure (yarp::os::ResourceFinder &rf) override
+
+
+bool updateModule () override
+
+
+bool interruptModule () override
+
+
+bool close () override
+
+
+double getPeriod () override
+
+
+
+
+GrabberResponder grabberResponder
+
+
+yarp::os::BufferedPort< yarp::os::Bottle > grabberPort
+
+
+yarp::dev::PolyDriver cartesianControlDevice
+
+
+roboticslab::ICartesianControl * iCartesianControl
+
+
+yarp::dev::PolyDriver sensorsClientDevice
+
+
+roboticslab::IProximitySensors * iProximitySensors
+
+
+double period
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1HaarDetectionController.png b/classroboticslab_1_1HaarDetectionController.png
new file mode 100644
index 000000000..58fd767c9
Binary files /dev/null and b/classroboticslab_1_1HaarDetectionController.png differ
diff --git a/classroboticslab_1_1ICartesianControl-members.html b/classroboticslab_1_1ICartesianControl-members.html
new file mode 100644
index 000000000..76c6591cb
--- /dev/null
+++ b/classroboticslab_1_1ICartesianControl-members.html
@@ -0,0 +1,109 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ICartesianControl , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1ICartesianControl.html b/classroboticslab_1_1ICartesianControl.html
new file mode 100644
index 000000000..7bbebc165
--- /dev/null
+++ b/classroboticslab_1_1ICartesianControl.html
@@ -0,0 +1,907 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ICartesianControl Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Abstract base class for a cartesian controller.
+
+
+
#include <ICartesianControl.h >
+
+
+
+
+
+
+
+
+
+
+
+
+virtual ~ICartesianControl ()=default
+ Destructor.
+
+
+
+
RPC commands with success/failure response.
+
+virtual bool stat (std::vector< double > &x, int *state=nullptr, double *timestamp=nullptr)=0
+ Current state and position. More...
+
+virtual bool inv (const std::vector< double > &xd, std::vector< double > &q)=0
+ Inverse kinematics. More...
+
+virtual bool movj (const std::vector< double > &xd)=0
+ Move in joint space, absolute coordinates. More...
+
+virtual bool relj (const std::vector< double > &xd)=0
+ Move in joint space, relative coordinates. More...
+
+virtual bool movl (const std::vector< double > &xd)=0
+ Linear move to target position. More...
+
+virtual bool movv (const std::vector< double > &xdotd)=0
+ Linear move with given velocity. More...
+
+virtual bool gcmp ()=0
+ Gravity compensation. More...
+
+virtual bool forc (const std::vector< double > &fd)=0
+ Force control. More...
+
+virtual bool stopControl ()=0
+ Stop control. More...
+
+virtual bool wait (double timeout=0.0)=0
+ Wait until completion. More...
+
+virtual bool tool (const std::vector< double > &x)=0
+ Change tool. More...
+
+virtual bool act (int command)=0
+ Actuate tool. More...
+
+
+
+
High-frequency streaming commands, no acknowledge.
+
+virtual void pose (const std::vector< double > &x)=0
+ Achieve pose. More...
+
+
+virtual void movi (const std::vector< double > &x)
+
+virtual void twist (const std::vector< double > &xdot)=0
+ Instantaneous velocity steps. More...
+
+virtual void wrench (const std::vector< double > &w)=0
+ Exert force. More...
+
+
+
+
Configuration setters and getters with success/failure response.
+
+virtual bool setParameter (int vocab, double value)=0
+ Set a configuration parameter. More...
+
+virtual bool getParameter (int vocab, double *value)=0
+ Retrieve a configuration parameter. More...
+
+virtual bool setParameters (const std::map< int, double > ¶ms)=0
+ Set multiple configuration parameters. More...
+
+virtual bool getParameters (std::map< int, double > ¶ms)=0
+ Retrieve multiple configuration parameters. More...
+
+
+
+
+
◆ act()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::act
+ (
+ int
+ command )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ forc()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::forc
+ (
+ const std::vector< double > &
+ fd )
+
+
+
+
+
+pure virtual
+
+
+
+
Apply desired forces in task space.
+
Parameters
+
+ fd 6-element vector describing desired force exerted by the TCP in cartesian space; first three elements denote linear force (Newton), last three denote torque (Newton*meters).
+
+
+
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::CartesianControlClient , and roboticslab::BasicCartesianControl .
+
+
+
+
+
◆ gcmp()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::gcmp
+ (
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ getParameter()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::getParameter
+ (
+ int
+ vocab ,
+
+
+
+
+ double *
+ value
+
+
+
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ getParameters()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::getParameters
+ (
+ std::map< int, double > &
+ params )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ inv()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::inv
+ (
+ const std::vector< double > &
+ xd ,
+
+
+
+
+ std::vector< double > &
+ q
+
+
+
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
Perform inverse kinematics (using robot position as initial guess), but do not move.
+
Parameters
+
+ xd 6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ q Vector describing current position in joint space (meters or degrees).
+
+
+
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::CartesianControlClient , and roboticslab::BasicCartesianControl .
+
+
+
+
+
◆ movj()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::movj
+ (
+ const std::vector< double > &
+ xd )
+
+
+
+
+
+pure virtual
+
+
+
+
Perform inverse kinematics and move to desired position in joint space using absolute coordinates.
+
Parameters
+
+ xd 6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
See also relj (relative coordinates)
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::CartesianControlClient , and roboticslab::BasicCartesianControl .
+
+
+
+
+
◆ movl()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::movl
+ (
+ const std::vector< double > &
+ xd )
+
+
+
+
+
+pure virtual
+
+
+
+
Move to end position along a line trajectory.
+
Parameters
+
+ xd 6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::CartesianControlClient , and roboticslab::BasicCartesianControl .
+
+
+
+
+
◆ movv()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::movv
+ (
+ const std::vector< double > &
+ xdotd )
+
+
+
+
+
+pure virtual
+
+
+
+
Move along a line with constant velocity.
+
Parameters
+
+ xdotd 6-element vector describing desired velocity in cartesian space; first three elements denote translational velocity (meters/second), last three denote angular velocity (radians/second).
+
+
+
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::CartesianControlClient , and roboticslab::BasicCartesianControl .
+
+
+
+
+
◆ pose()
+
+
+
+
+
+
+
+
+ virtual void roboticslab::ICartesianControl::pose
+ (
+ const std::vector< double > &
+ x )
+
+
+
+
+
+pure virtual
+
+
+
+
Move to desired position instantaneously, no further intermediate calculations are expected other than computing the inverse kinematics.
+
Parameters
+
+ x 6-element vector describing desired instantaneous pose in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
+
Implemented in roboticslab::CartesianControlClient , and roboticslab::BasicCartesianControl .
+
+
+
+
+
◆ relj()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::relj
+ (
+ const std::vector< double > &
+ xd )
+
+
+
+
+
+pure virtual
+
+
+
+
Perform inverse kinematics and move to desired position in joint space using relative coordinates.
+
Parameters
+
+ xd 6-element vector describing desired offset in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
See also movj (absolute coordinates)
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::CartesianControlClient , and roboticslab::BasicCartesianControl .
+
+
+
+
+
◆ setParameter()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::setParameter
+ (
+ int
+ vocab ,
+
+
+
+
+ double
+ value
+
+
+
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ setParameters()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::setParameters
+ (
+ const std::map< int, double > &
+ params )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ stat()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::stat
+ (
+ std::vector< double > &
+ x ,
+
+
+
+
+ int *
+ state = nullptr
,
+
+
+
+
+ double *
+ timestamp = nullptr
+
+
+
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
Inform on control state, get robot position and perform forward kinematics.
+
Parameters
+
+ x 6-element vector describing current position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ state Identifier for a cartesian control vocab.
+ timestamp Remote encoder acquisition time.
+
+
+
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::CartesianControlClient , and roboticslab::BasicCartesianControl .
+
+
+
+
+
◆ stopControl()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::stopControl
+ (
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ tool()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::tool
+ (
+ const std::vector< double > &
+ x )
+
+
+
+
+
+pure virtual
+
+
+
+
Unload current tool if any and append new tool frame to the kinematic chain.
+
Parameters
+
+ x 6-element vector describing new tool tip with regard to current end-effector frame in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::CartesianControlClient , and roboticslab::BasicCartesianControl .
+
+
+
+
+
◆ twist()
+
+
+
+
+
+
+
+
+ virtual void roboticslab::ICartesianControl::twist
+ (
+ const std::vector< double > &
+ xdot )
+
+
+
+
+
+pure virtual
+
+
+
+
Move in instantaneous velocity increments.
+
Parameters
+
+ xdot 6-element vector describing velocity increments in cartesian space; first three elements denote translational velocity (meters/second), last three denote angular velocity (radians/second).
+
+
+
+
+
Implemented in roboticslab::CartesianControlClient , and roboticslab::BasicCartesianControl .
+
+
+
+
+
◆ wait()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianControl::wait
+ (
+ double
+ timeout = 0.0
)
+
+
+
+
+
+pure virtual
+
+
+
+
Block execution until the movement is completed, errors occur or timeout is reached.
+
Parameters
+
+ timeout Timeout in seconds, '0.0' means no timeout.
+
+
+
+
Returns true on success, false if errors occurred during the execution of the trajectory
+
+
Implemented in roboticslab::CartesianControlClient , and roboticslab::BasicCartesianControl .
+
+
+
+
+
◆ wrench()
+
+
+
+
+
+
+
+
+ virtual void roboticslab::ICartesianControl::wrench
+ (
+ const std::vector< double > &
+ w )
+
+
+
+
+
+pure virtual
+
+
+
+
Make the TCP exert the desired force instantaneously.
+
Parameters
+
+ w 6-element vector describing desired force exerted by the TCP in cartesian space; first three elements denote linear force (Newton), last three denote torque (Newton*meters).
+
+
+
+
+
Implemented in roboticslab::CartesianControlClient , and roboticslab::BasicCartesianControl .
+
+
+
+
The documentation for this class was generated from the following file:
+
+
+
+
+
diff --git a/classroboticslab_1_1ICartesianControl.png b/classroboticslab_1_1ICartesianControl.png
new file mode 100644
index 000000000..77f81919b
Binary files /dev/null and b/classroboticslab_1_1ICartesianControl.png differ
diff --git a/classroboticslab_1_1ICartesianSolver-members.html b/classroboticslab_1_1ICartesianSolver-members.html
new file mode 100644
index 000000000..7e038d964
--- /dev/null
+++ b/classroboticslab_1_1ICartesianSolver-members.html
@@ -0,0 +1,104 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ICartesianSolver , including all inherited members.
+
+ appendLink (const std::vector< double > &x)=0roboticslab::ICartesianSolver pure virtual
+ BASE_FRAME enum valueroboticslab::ICartesianSolver
+ changeOrigin (const std::vector< double > &x_old_obj, const std::vector< double > &x_new_old, std::vector< double > &x_new_obj)=0roboticslab::ICartesianSolver pure virtual
+ diffInvKin (const std::vector< double > &q, const std::vector< double > &xdot, std::vector< double > &qdot, reference_frame frame=BASE_FRAME)=0roboticslab::ICartesianSolver pure virtual
+ fwdKin (const std::vector< double > &q, std::vector< double > &x)=0roboticslab::ICartesianSolver pure virtual
+ getNumJoints ()=0roboticslab::ICartesianSolver pure virtual
+ getNumTcps ()=0roboticslab::ICartesianSolver pure virtual
+ invDyn (const std::vector< double > &q, std::vector< double > &t)=0roboticslab::ICartesianSolver pure virtual
+ invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< std::vector< double >> &fexts, std::vector< double > &t)roboticslab::ICartesianSolver inline virtual
+ invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< double > &ftip, std::vector< double > &t, reference_frame frame=BASE_FRAME)=0roboticslab::ICartesianSolver pure virtual
+ invKin (const std::vector< double > &xd, const std::vector< double > &qGuess, std::vector< double > &q, reference_frame frame=BASE_FRAME)=0roboticslab::ICartesianSolver pure virtual
+ poseDiff (const std::vector< double > &xLhs, const std::vector< double > &xRhs, std::vector< double > &xOut)=0roboticslab::ICartesianSolver pure virtual
+ reference_frame enum nameroboticslab::ICartesianSolver
+ restoreOriginalChain ()=0roboticslab::ICartesianSolver pure virtual
+ TCP_FRAME enum valueroboticslab::ICartesianSolver
+ ~ICartesianSolver ()=defaultroboticslab::ICartesianSolver virtual
+
+
+
+
+
diff --git a/classroboticslab_1_1ICartesianSolver.html b/classroboticslab_1_1ICartesianSolver.html
new file mode 100644
index 000000000..3691a82b3
--- /dev/null
+++ b/classroboticslab_1_1ICartesianSolver.html
@@ -0,0 +1,760 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ICartesianSolver Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Abstract base class for a cartesian solver.
+
+
+
#include <ICartesianSolver.h >
+
+
+
+
+
+
+
+
+
+
+
+
+
+virtual ~ICartesianSolver ()=default
+ Destructor.
+
+virtual int getNumJoints ()=0
+ Get number of joints for which the solver has been configured. More...
+
+virtual int getNumTcps ()=0
+ Get number of TCPs for which the solver has been configured. More...
+
+virtual bool appendLink (const std::vector< double > &x)=0
+ Append an additional link. More...
+
+virtual bool restoreOriginalChain ()=0
+ Restore original kinematic chain. More...
+
+virtual bool changeOrigin (const std::vector< double > &x_old_obj, const std::vector< double > &x_new_old, std::vector< double > &x_new_obj)=0
+ Change origin in which a pose is expressed. More...
+
+virtual bool fwdKin (const std::vector< double > &q, std::vector< double > &x)=0
+ Perform forward kinematics. More...
+
+virtual bool poseDiff (const std::vector< double > &xLhs, const std::vector< double > &xRhs, std::vector< double > &xOut)=0
+ Obtain difference between supplied pose inputs. More...
+
+virtual bool invKin (const std::vector< double > &xd, const std::vector< double > &qGuess, std::vector< double > &q, reference_frame frame=BASE_FRAME )=0
+ Perform inverse kinematics. More...
+
+virtual bool diffInvKin (const std::vector< double > &q, const std::vector< double > &xdot, std::vector< double > &qdot, reference_frame frame=BASE_FRAME )=0
+ Perform differential inverse kinematics. More...
+
+virtual bool invDyn (const std::vector< double > &q, std::vector< double > &t)=0
+ Perform inverse dynamics. More...
+
+virtual bool invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< std::vector< double >> &fexts, std::vector< double > &t)
+ Perform inverse dynamics. More...
+
+virtual bool invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< double > &ftip, std::vector< double > &t, reference_frame frame=BASE_FRAME )=0
+ Perform inverse dynamics. More...
+
+
+
+
+
◆ reference_frame
+
+
+
+
+Enumerator BASE_FRAME Base frame.
+
+ TCP_FRAME End-effector frame (TCP)
+
+
+
+
+
+
+
+
◆ appendLink()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianSolver::appendLink
+ (
+ const std::vector< double > &
+ x )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ changeOrigin()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianSolver::changeOrigin
+ (
+ const std::vector< double > &
+ x_old_obj ,
+
+
+
+
+ const std::vector< double > &
+ x_new_old ,
+
+
+
+
+ std::vector< double > &
+ x_new_obj
+
+
+
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
Parameters
+
+ x_old_obj_in 6-element vector describing a pose in cartesian space, expressed in the old frame; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ x_new_old 6-element vector describing a transformation from the new to the old frame; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ x_new_obj 6-element vector describing a pose in cartesian space, expressed in the new frame; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::KdlTreeSolver , roboticslab::KdlSolver , and roboticslab::AsibotSolver .
+
+
+
+
+
◆ diffInvKin()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianSolver::diffInvKin
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ const std::vector< double > &
+ xdot ,
+
+
+
+
+ std::vector< double > &
+ qdot ,
+
+
+
+
+ reference_frame
+ frame = BASE_FRAME
+
+
+
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
Parameters
+
+ q Vector describing current position in joint space (meters or degrees).
+ xdot 6-element vector describing desired velocity in cartesian space; first three elements denote translational velocity (meters/second), last three denote angular velocity (radians/second).
+ qdot Vector describing target velocity in joint space (meters/second or degrees/second).
+ frame Points at the reference_frame the desired position is expressed in.
+
+
+
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::KdlTreeSolver , roboticslab::KdlSolver , and roboticslab::AsibotSolver .
+
+
+
+
+
◆ fwdKin()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianSolver::fwdKin
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ std::vector< double > &
+ x
+
+
+
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
Parameters
+
+ q Vector describing a position in joint space (meters or degrees).
+ x 6-element vector describing same position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::KdlTreeSolver , roboticslab::KdlSolver , and roboticslab::AsibotSolver .
+
+
+
+
+
◆ getNumJoints()
+
+
+
+
+
+
+
+
+ virtual int roboticslab::ICartesianSolver::getNumJoints
+ (
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ getNumTcps()
+
+
+
+
+
+
+
+
+ virtual int roboticslab::ICartesianSolver::getNumTcps
+ (
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ invDyn() [1/3]
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianSolver::invDyn
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ const std::vector< double > &
+ qdot ,
+
+
+
+
+ const std::vector< double > &
+ qdotdot ,
+
+
+
+
+ const std::vector< double > &
+ ftip ,
+
+
+
+
+ std::vector< double > &
+ t ,
+
+
+
+
+ reference_frame
+ frame = BASE_FRAME
+
+
+
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
Parameters
+
+ q Vector describing current position in joint space (meters or degrees).
+ qdot Vector describing current velocity in joint space (meters/second or degrees/second).
+ qdotdot Vector describing current acceleration in joint space (meters/second² or degrees/second²).
+ ftip Vector describing an external force applied to the robot tip, expressed in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+ t 6-element vector describing desired forces in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+ frame Points at the reference_frame ftip
is expressed in.
+
+
+
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::KdlTreeSolver , roboticslab::KdlSolver , and roboticslab::AsibotSolver .
+
+
+
+
+
◆ invDyn() [2/3]
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianSolver::invDyn
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ const std::vector< double > &
+ qdot ,
+
+
+
+
+ const std::vector< double > &
+ qdotdot ,
+
+
+
+
+ const std::vector< std::vector< double >> &
+ fexts ,
+
+
+
+
+ std::vector< double > &
+ t
+
+
+
+ )
+
+
+
+
+
+inline virtual
+
+
+
+
Parameters
+
+ q Vector describing current position in joint space (meters or degrees).
+ qdot Vector describing current velocity in joint space (meters/second or degrees/second).
+ qdotdot Vector describing current acceleration in joint space (meters/second² or degrees/second²).
+ fexts vector of external forces applied to each robot segment, expressed in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+ t 6-element vector describing desired forces in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+
+
+
+
Returns true on success, false otherwise
+
+
+
+
+
◆ invDyn() [3/3]
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianSolver::invDyn
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ std::vector< double > &
+ t
+
+
+
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
Assumes null joint velocities and accelerations, and no external forces.
+
Parameters
+
+ q Vector describing current position in joint space (meters or degrees).
+ t 6-element vector describing desired forces in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+
+
+
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::KdlTreeSolver , roboticslab::KdlSolver , and roboticslab::AsibotSolver .
+
+
+
+
+
◆ invKin()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianSolver::invKin
+ (
+ const std::vector< double > &
+ xd ,
+
+
+
+
+ const std::vector< double > &
+ qGuess ,
+
+
+
+
+ std::vector< double > &
+ q ,
+
+
+
+
+ reference_frame
+ frame = BASE_FRAME
+
+
+
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
Parameters
+
+ xd 6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ qGuess Vector describing current position in joint space (meters or degrees).
+ q Vector describing target position in joint space (meters or degrees).
+ frame Points at the reference_frame the desired position is expressed in.
+
+
+
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::KdlTreeSolver , roboticslab::KdlSolver , and roboticslab::AsibotSolver .
+
+
+
+
+
◆ poseDiff()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianSolver::poseDiff
+ (
+ const std::vector< double > &
+ xLhs ,
+
+
+
+
+ const std::vector< double > &
+ xRhs ,
+
+
+
+
+ std::vector< double > &
+ xOut
+
+
+
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
The result is an infinitesimal displacement twist, i.e. a vector, for which the operation of addition makes physical sense.
+
Parameters
+
+ xLhs 6-element vector describing a pose in cartesian space (left hand side); first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ xRhs 6-element vector describing a pose in cartesian space (right hand side); first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ xOut 6-element vector describing a pose in cartesian space (result); first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implemented in roboticslab::KdlTreeSolver , roboticslab::KdlSolver , and roboticslab::AsibotSolver .
+
+
+
+
+
◆ restoreOriginalChain()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ICartesianSolver::restoreOriginalChain
+ (
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
+
The documentation for this class was generated from the following file:
+
+
+
+
+
diff --git a/classroboticslab_1_1ICartesianSolver.png b/classroboticslab_1_1ICartesianSolver.png
new file mode 100644
index 000000000..3e1c41f95
Binary files /dev/null and b/classroboticslab_1_1ICartesianSolver.png differ
diff --git a/classroboticslab_1_1InvalidDevice-members.html b/classroboticslab_1_1InvalidDevice-members.html
new file mode 100644
index 000000000..b03404cf3
--- /dev/null
+++ b/classroboticslab_1_1InvalidDevice-members.html
@@ -0,0 +1,106 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::InvalidDevice , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1InvalidDevice.html b/classroboticslab_1_1InvalidDevice.html
new file mode 100644
index 000000000..024d52eb0
--- /dev/null
+++ b/classroboticslab_1_1InvalidDevice.html
@@ -0,0 +1,258 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::InvalidDevice Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Represents an invalid device.
+ More...
+
+
#include <StreamingDevice.hpp >
+
+
+
+
+
+
+
+
+
+
+
+
+ICartesianControl * iCartesianControl
+
+
+std::vector< double > data
+
+
+std::vector< bool > fixedAxes
+
+
+int actuatorState
+
+
+
+
A call to isValid() and other interface methods should yield false.
+
+
+
◆ acquireData()
+
+
+
+
+
+
+
+
+ bool roboticslab::InvalidDevice::acquireData
+ (
+ )
+
+
+
+
+
+inline override virtual
+
+
+
+
+
+
◆ acquireInterfaces()
+
+
+
+
+
+
+
+
+ bool roboticslab::InvalidDevice::acquireInterfaces
+ (
+ )
+
+
+
+
+
+inline override virtual
+
+
+
+
+
+
◆ sendMovementCommand()
+
+
+
+
+
+
+
+
+ void roboticslab::InvalidDevice::sendMovementCommand
+ (
+ double
+ timestamp )
+
+
+
+
+
+inline override virtual
+
+
+
+
+
The documentation for this class was generated from the following file:
+
+
+
+
+
diff --git a/classroboticslab_1_1InvalidDevice.png b/classroboticslab_1_1InvalidDevice.png
new file mode 100644
index 000000000..7a02c2dbe
Binary files /dev/null and b/classroboticslab_1_1InvalidDevice.png differ
diff --git a/classroboticslab_1_1KdlSolver-members.html b/classroboticslab_1_1KdlSolver-members.html
new file mode 100644
index 000000000..d76329a58
--- /dev/null
+++ b/classroboticslab_1_1KdlSolver-members.html
@@ -0,0 +1,115 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::KdlSolver , including all inherited members.
+
+ appendLink (const std::vector< double > &x) overrideroboticslab::KdlSolver virtual
+ BASE_FRAME enum valueroboticslab::ICartesianSolver
+ chain roboticslab::KdlSolver private
+ changeOrigin (const std::vector< double > &x_old_obj, const std::vector< double > &x_new_old, std::vector< double > &x_new_obj) overrideroboticslab::KdlSolver virtual
+ close () override (defined in roboticslab::KdlSolver )roboticslab::KdlSolver
+ diffInvKin (const std::vector< double > &q, const std::vector< double > &xdot, std::vector< double > &qdot, reference_frame frame) overrideroboticslab::KdlSolver virtual
+ fkSolverPos (defined in roboticslab::KdlSolver )roboticslab::KdlSolver private
+ fwdKin (const std::vector< double > &q, std::vector< double > &x) overrideroboticslab::KdlSolver virtual
+ getNumJoints () overrideroboticslab::KdlSolver virtual
+ getNumTcps () overrideroboticslab::KdlSolver virtual
+ idSolver (defined in roboticslab::KdlSolver )roboticslab::KdlSolver private
+ ikSolverPos (defined in roboticslab::KdlSolver )roboticslab::KdlSolver private
+ ikSolverVel (defined in roboticslab::KdlSolver )roboticslab::KdlSolver private
+ invDyn (const std::vector< double > &q, std::vector< double > &t) overrideroboticslab::KdlSolver virtual
+ invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< double > &ftip, std::vector< double > &t, reference_frame frame) overrideroboticslab::KdlSolver virtual
+ roboticslab::ICartesianSolver::invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< std::vector< double >> &fexts, std::vector< double > &t)roboticslab::ICartesianSolver inline virtual
+ invKin (const std::vector< double > &xd, const std::vector< double > &qGuess, std::vector< double > &q, reference_frame frame) overrideroboticslab::KdlSolver virtual
+ isQuiet (defined in roboticslab::KdlSolver )roboticslab::KdlSolver private
+ logc () const (defined in roboticslab::KdlSolver )roboticslab::KdlSolver inline private
+ mtx (defined in roboticslab::KdlSolver )roboticslab::KdlSolver mutable private
+ open (yarp::os::Searchable &config) override (defined in roboticslab::KdlSolver )roboticslab::KdlSolver
+ originalChain roboticslab::KdlSolver private
+ poseDiff (const std::vector< double > &xLhs, const std::vector< double > &xRhs, std::vector< double > &xOut) overrideroboticslab::KdlSolver virtual
+ reference_frame enum nameroboticslab::ICartesianSolver
+ restoreOriginalChain () overrideroboticslab::KdlSolver virtual
+ TCP_FRAME enum valueroboticslab::ICartesianSolver
+ ~ICartesianSolver ()=defaultroboticslab::ICartesianSolver virtual
+
+
+
+
+
diff --git a/classroboticslab_1_1KdlSolver.html b/classroboticslab_1_1KdlSolver.html
new file mode 100644
index 000000000..3b676662e
--- /dev/null
+++ b/classroboticslab_1_1KdlSolver.html
@@ -0,0 +1,761 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::KdlSolver Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
The KdlSolver class implements ICartesianSolver .
+
+
+
#include <KdlSolver.hpp >
+
+
+
+
+
+
+
+
+
+
+int getNumJoints () override
+ Get number of joints for which the solver has been configured. More...
+
+int getNumTcps () override
+ Get number of TCPs for which the solver has been configured. More...
+
+bool appendLink (const std::vector< double > &x) override
+ Append an additional link. More...
+
+bool restoreOriginalChain () override
+ Restore original kinematic chain. More...
+
+bool changeOrigin (const std::vector< double > &x_old_obj, const std::vector< double > &x_new_old, std::vector< double > &x_new_obj) override
+ Change origin in which a pose is expressed. More...
+
+bool fwdKin (const std::vector< double > &q, std::vector< double > &x) override
+ Perform forward kinematics. More...
+
+bool poseDiff (const std::vector< double > &xLhs, const std::vector< double > &xRhs, std::vector< double > &xOut) override
+ Obtain difference between supplied pose inputs. More...
+
+bool invKin (const std::vector< double > &xd, const std::vector< double > &qGuess, std::vector< double > &q, reference_frame frame) override
+ Perform inverse kinematics. More...
+
+bool diffInvKin (const std::vector< double > &q, const std::vector< double > &xdot, std::vector< double > &qdot, reference_frame frame) override
+ Perform differential inverse kinematics. More...
+
+bool invDyn (const std::vector< double > &q, std::vector< double > &t) override
+ Perform inverse dynamics. More...
+
+bool invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< double > &ftip, std::vector< double > &t, reference_frame frame) override
+ Perform inverse dynamics. More...
+
+
+bool open (yarp::os::Searchable &config) override
+
+
+bool close () override
+
+
+
+virtual ~ICartesianSolver ()=default
+ Destructor.
+
+virtual bool invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< std::vector< double >> &fexts, std::vector< double > &t)
+ Perform inverse dynamics. More...
+
+
+
+
+const yarp::os::LogComponent & logc () const
+
+
+
+
+std::mutex mtx
+
+KDL::Chain chain
+
+KDL::Chain originalChain
+
+
+KDL::ChainFkSolverPos * fkSolverPos {nullptr}
+
+
+KDL::ChainIkSolverPos * ikSolverPos {nullptr}
+
+
+KDL::ChainIkSolverVel * ikSolverVel {nullptr}
+
+
+KDL::ChainIdSolver * idSolver {nullptr}
+
+
+bool isQuiet
+
+
+
+
+
◆ appendLink()
+
+
+
+
+
+
+
+
+ bool KdlSolver::appendLink
+ (
+ const std::vector< double > &
+ x )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ x 6-element vector describing end-effector frame in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ changeOrigin()
+
+
+
+
+
+
+
+
+ bool KdlSolver::changeOrigin
+ (
+ const std::vector< double > &
+ x_old_obj ,
+
+
+
+
+ const std::vector< double > &
+ x_new_old ,
+
+
+
+
+ std::vector< double > &
+ x_new_obj
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ x_old_obj_in 6-element vector describing a pose in cartesian space, expressed in the old frame; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ x_new_old 6-element vector describing a transformation from the new to the old frame; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ x_new_obj 6-element vector describing a pose in cartesian space, expressed in the new frame; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ diffInvKin()
+
+
+
+
+
+
+
+
+ bool KdlSolver::diffInvKin
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ const std::vector< double > &
+ xdot ,
+
+
+
+
+ std::vector< double > &
+ qdot ,
+
+
+
+
+ reference_frame
+ frame
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ q Vector describing current position in joint space (meters or degrees).
+ xdot 6-element vector describing desired velocity in cartesian space; first three elements denote translational velocity (meters/second), last three denote angular velocity (radians/second).
+ qdot Vector describing target velocity in joint space (meters/second or degrees/second).
+ frame Points at the reference_frame the desired position is expressed in.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ fwdKin()
+
+
+
+
+
+
+
+
+ bool KdlSolver::fwdKin
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ std::vector< double > &
+ x
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ q Vector describing a position in joint space (meters or degrees).
+ x 6-element vector describing same position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ getNumJoints()
+
+
+
+
+
+
+
+
+ int KdlSolver::getNumJoints
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ getNumTcps()
+
+
+
+
+
+
+
+
+ int KdlSolver::getNumTcps
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ invDyn() [1/2]
+
+
+
+
+
+
+
+
+ bool KdlSolver::invDyn
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ const std::vector< double > &
+ qdot ,
+
+
+
+
+ const std::vector< double > &
+ qdotdot ,
+
+
+
+
+ const std::vector< double > &
+ ftip ,
+
+
+
+
+ std::vector< double > &
+ t ,
+
+
+
+
+ reference_frame
+ frame
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ q Vector describing current position in joint space (meters or degrees).
+ qdot Vector describing current velocity in joint space (meters/second or degrees/second).
+ qdotdot Vector describing current acceleration in joint space (meters/second² or degrees/second²).
+ ftip Vector describing an external force applied to the robot tip, expressed in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+ t 6-element vector describing desired forces in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+ frame Points at the reference_frame ftip
is expressed in.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ invDyn() [2/2]
+
+
+
+
+
+
+
+
+ bool KdlSolver::invDyn
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ std::vector< double > &
+ t
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Assumes null joint velocities and accelerations, and no external forces.
+
Parameters
+
+ q Vector describing current position in joint space (meters or degrees).
+ t 6-element vector describing desired forces in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ invKin()
+
+
+
+
+
+
+
+
+ bool KdlSolver::invKin
+ (
+ const std::vector< double > &
+ xd ,
+
+
+
+
+ const std::vector< double > &
+ qGuess ,
+
+
+
+
+ std::vector< double > &
+ q ,
+
+
+
+
+ reference_frame
+ frame
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ xd 6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ qGuess Vector describing current position in joint space (meters or degrees).
+ q Vector describing target position in joint space (meters or degrees).
+ frame Points at the reference_frame the desired position is expressed in.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ poseDiff()
+
+
+
+
+
+
+
+
+ bool KdlSolver::poseDiff
+ (
+ const std::vector< double > &
+ xLhs ,
+
+
+
+
+ const std::vector< double > &
+ xRhs ,
+
+
+
+
+ std::vector< double > &
+ xOut
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
The result is an infinitesimal displacement twist, i.e. a vector, for which the operation of addition makes physical sense.
+
Parameters
+
+ xLhs 6-element vector describing a pose in cartesian space (left hand side); first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ xRhs 6-element vector describing a pose in cartesian space (right hand side); first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ xOut 6-element vector describing a pose in cartesian space (result); first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ restoreOriginalChain()
+
+
+
+
+
+
+
+
+ bool KdlSolver::restoreOriginalChain
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
+
◆ chain
+
+
+
+
+
+
+
+
+ KDL::Chain roboticslab::KdlSolver::chain
+
+
+
+
+private
+
+
+
+
+
+
◆ originalChain
+
+
+
+
+
+
+
+
+ KDL::Chain roboticslab::KdlSolver::originalChain
+
+
+
+
+private
+
+
+
+
To store a copy of the original chain.
+
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/KdlSolver/KdlSolver.hpp
+libraries/YarpPlugins/KdlSolver/DeviceDriverImpl.cpp
+libraries/YarpPlugins/KdlSolver/ICartesianSolverImpl.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1KdlSolver.png b/classroboticslab_1_1KdlSolver.png
new file mode 100644
index 000000000..4769dcbf6
Binary files /dev/null and b/classroboticslab_1_1KdlSolver.png differ
diff --git a/classroboticslab_1_1KdlTreeSolver-members.html b/classroboticslab_1_1KdlTreeSolver-members.html
new file mode 100644
index 000000000..77646f060
--- /dev/null
+++ b/classroboticslab_1_1KdlTreeSolver-members.html
@@ -0,0 +1,114 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::KdlTreeSolver , including all inherited members.
+
+ appendLink (const std::vector< double > &x) overrideroboticslab::KdlTreeSolver virtual
+ BASE_FRAME enum valueroboticslab::ICartesianSolver
+ changeOrigin (const std::vector< double > &x_old_obj, const std::vector< double > &x_new_old, std::vector< double > &x_new_obj) overrideroboticslab::KdlTreeSolver virtual
+ close () override (defined in roboticslab::KdlTreeSolver )roboticslab::KdlTreeSolver
+ diffInvKin (const std::vector< double > &q, const std::vector< double > &xdot, std::vector< double > &qdot, reference_frame frame) overrideroboticslab::KdlTreeSolver virtual
+ endpoints (defined in roboticslab::KdlTreeSolver )roboticslab::KdlTreeSolver protected
+ fkSolverPos (defined in roboticslab::KdlTreeSolver )roboticslab::KdlTreeSolver protected
+ fwdKin (const std::vector< double > &q, std::vector< double > &x) overrideroboticslab::KdlTreeSolver virtual
+ getNumJoints () overrideroboticslab::KdlTreeSolver virtual
+ getNumTcps () overrideroboticslab::KdlTreeSolver virtual
+ idSolver (defined in roboticslab::KdlTreeSolver )roboticslab::KdlTreeSolver protected
+ ikSolverPos (defined in roboticslab::KdlTreeSolver )roboticslab::KdlTreeSolver protected
+ ikSolverVel (defined in roboticslab::KdlTreeSolver )roboticslab::KdlTreeSolver protected
+ invDyn (const std::vector< double > &q, std::vector< double > &t) overrideroboticslab::KdlTreeSolver virtual
+ invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< double > &ftip, std::vector< double > &t, reference_frame frame) overrideroboticslab::KdlTreeSolver virtual
+ roboticslab::ICartesianSolver::invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< std::vector< double >> &fexts, std::vector< double > &t)roboticslab::ICartesianSolver inline virtual
+ invKin (const std::vector< double > &xd, const std::vector< double > &qGuess, std::vector< double > &q, reference_frame frame) overrideroboticslab::KdlTreeSolver virtual
+ KdlTreeSolver () (defined in roboticslab::KdlTreeSolver )roboticslab::KdlTreeSolver inline
+ mergedEndpoints (defined in roboticslab::KdlTreeSolver )roboticslab::KdlTreeSolver protected
+ open (yarp::os::Searchable &config) override (defined in roboticslab::KdlTreeSolver )roboticslab::KdlTreeSolver
+ poseDiff (const std::vector< double > &xLhs, const std::vector< double > &xRhs, std::vector< double > &xOut) overrideroboticslab::KdlTreeSolver virtual
+ reference_frame enum nameroboticslab::ICartesianSolver
+ restoreOriginalChain () overrideroboticslab::KdlTreeSolver virtual
+ TCP_FRAME enum valueroboticslab::ICartesianSolver
+ tree (defined in roboticslab::KdlTreeSolver )roboticslab::KdlTreeSolver protected
+ ~ICartesianSolver ()=defaultroboticslab::ICartesianSolver virtual
+
+
+
+
+
diff --git a/classroboticslab_1_1KdlTreeSolver.html b/classroboticslab_1_1KdlTreeSolver.html
new file mode 100644
index 000000000..8bc4074e8
--- /dev/null
+++ b/classroboticslab_1_1KdlTreeSolver.html
@@ -0,0 +1,706 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::KdlTreeSolver Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
The KdlTreeSolver class implements ICartesianSolver .
+
+
+
#include <KdlTreeSolver.hpp >
+
+
+
+
+
+
+
+
+
+
+int getNumJoints () override
+ Get number of joints for which the solver has been configured. More...
+
+int getNumTcps () override
+ Get number of TCPs for which the solver has been configured. More...
+
+bool appendLink (const std::vector< double > &x) override
+ Append an additional link. More...
+
+bool restoreOriginalChain () override
+ Restore original kinematic chain. More...
+
+bool changeOrigin (const std::vector< double > &x_old_obj, const std::vector< double > &x_new_old, std::vector< double > &x_new_obj) override
+ Change origin in which a pose is expressed. More...
+
+bool fwdKin (const std::vector< double > &q, std::vector< double > &x) override
+ Perform forward kinematics. More...
+
+bool poseDiff (const std::vector< double > &xLhs, const std::vector< double > &xRhs, std::vector< double > &xOut) override
+ Obtain difference between supplied pose inputs. More...
+
+bool invKin (const std::vector< double > &xd, const std::vector< double > &qGuess, std::vector< double > &q, reference_frame frame) override
+ Perform inverse kinematics. More...
+
+bool diffInvKin (const std::vector< double > &q, const std::vector< double > &xdot, std::vector< double > &qdot, reference_frame frame) override
+ Perform differential inverse kinematics. More...
+
+bool invDyn (const std::vector< double > &q, std::vector< double > &t) override
+ Perform inverse dynamics. More...
+
+bool invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< double > &ftip, std::vector< double > &t, reference_frame frame) override
+ Perform inverse dynamics. More...
+
+
+bool open (yarp::os::Searchable &config) override
+
+
+bool close () override
+
+
+
+virtual ~ICartesianSolver ()=default
+ Destructor.
+
+virtual bool invDyn (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const std::vector< std::vector< double >> &fexts, std::vector< double > &t)
+ Perform inverse dynamics. More...
+
+
+
+
+std::vector< std::string > endpoints
+
+
+std::map< std::string, std::string > mergedEndpoints
+
+
+KDL::Tree tree
+
+
+KDL::TreeFkSolverPos * fkSolverPos
+
+
+KDL::TreeIkSolverPos * ikSolverPos
+
+
+KDL::TreeIkSolverVel * ikSolverVel
+
+
+KDL::TreeIdSolver * idSolver
+
+
+
+
+
◆ appendLink()
+
+
+
+
+
+
+
+
+ bool KdlTreeSolver::appendLink
+ (
+ const std::vector< double > &
+ x )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ x 6-element vector describing end-effector frame in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ changeOrigin()
+
+
+
+
+
+
+
+
+ bool KdlTreeSolver::changeOrigin
+ (
+ const std::vector< double > &
+ x_old_obj ,
+
+
+
+
+ const std::vector< double > &
+ x_new_old ,
+
+
+
+
+ std::vector< double > &
+ x_new_obj
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ x_old_obj_in 6-element vector describing a pose in cartesian space, expressed in the old frame; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ x_new_old 6-element vector describing a transformation from the new to the old frame; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ x_new_obj 6-element vector describing a pose in cartesian space, expressed in the new frame; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ diffInvKin()
+
+
+
+
+
+
+
+
+ bool KdlTreeSolver::diffInvKin
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ const std::vector< double > &
+ xdot ,
+
+
+
+
+ std::vector< double > &
+ qdot ,
+
+
+
+
+ reference_frame
+ frame
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ q Vector describing current position in joint space (meters or degrees).
+ xdot 6-element vector describing desired velocity in cartesian space; first three elements denote translational velocity (meters/second), last three denote angular velocity (radians/second).
+ qdot Vector describing target velocity in joint space (meters/second or degrees/second).
+ frame Points at the reference_frame the desired position is expressed in.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ fwdKin()
+
+
+
+
+
+
+
+
+ bool KdlTreeSolver::fwdKin
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ std::vector< double > &
+ x
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ q Vector describing a position in joint space (meters or degrees).
+ x 6-element vector describing same position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ getNumJoints()
+
+
+
+
+
+
+
+
+ int KdlTreeSolver::getNumJoints
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ getNumTcps()
+
+
+
+
+
+
+
+
+ int KdlTreeSolver::getNumTcps
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ invDyn() [1/2]
+
+
+
+
+
+
+
+
+ bool KdlTreeSolver::invDyn
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ const std::vector< double > &
+ qdot ,
+
+
+
+
+ const std::vector< double > &
+ qdotdot ,
+
+
+
+
+ const std::vector< double > &
+ ftip ,
+
+
+
+
+ std::vector< double > &
+ t ,
+
+
+
+
+ reference_frame
+ frame
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ q Vector describing current position in joint space (meters or degrees).
+ qdot Vector describing current velocity in joint space (meters/second or degrees/second).
+ qdotdot Vector describing current acceleration in joint space (meters/second² or degrees/second²).
+ ftip Vector describing an external force applied to the robot tip, expressed in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+ t 6-element vector describing desired forces in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+ frame Points at the reference_frame ftip
is expressed in.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ invDyn() [2/2]
+
+
+
+
+
+
+
+
+ bool KdlTreeSolver::invDyn
+ (
+ const std::vector< double > &
+ q ,
+
+
+
+
+ std::vector< double > &
+ t
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Assumes null joint velocities and accelerations, and no external forces.
+
Parameters
+
+ q Vector describing current position in joint space (meters or degrees).
+ t 6-element vector describing desired forces in cartesian space; first three elements denote translational acceleration (meters/second²), last three denote angular acceleration (radians/second²).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ invKin()
+
+
+
+
+
+
+
+
+ bool KdlTreeSolver::invKin
+ (
+ const std::vector< double > &
+ xd ,
+
+
+
+
+ const std::vector< double > &
+ qGuess ,
+
+
+
+
+ std::vector< double > &
+ q ,
+
+
+
+
+ reference_frame
+ frame
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ xd 6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ qGuess Vector describing current position in joint space (meters or degrees).
+ q Vector describing target position in joint space (meters or degrees).
+ frame Points at the reference_frame the desired position is expressed in.
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ poseDiff()
+
+
+
+
+
+
+
+
+ bool KdlTreeSolver::poseDiff
+ (
+ const std::vector< double > &
+ xLhs ,
+
+
+
+
+ const std::vector< double > &
+ xRhs ,
+
+
+
+
+ std::vector< double > &
+ xOut
+
+
+
+ )
+
+
+
+
+
+override virtual
+
+
+
+
The result is an infinitesimal displacement twist, i.e. a vector, for which the operation of addition makes physical sense.
+
Parameters
+
+ xLhs 6-element vector describing a pose in cartesian space (left hand side); first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ xRhs 6-element vector describing a pose in cartesian space (right hand side); first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+ xOut 6-element vector describing a pose in cartesian space (result); first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
+
+
+
+
Returns true on success, false otherwise
+
+
Implements roboticslab::ICartesianSolver .
+
+
+
+
+
◆ restoreOriginalChain()
+
+
+
+
+
+
+
+
+ bool KdlTreeSolver::restoreOriginalChain
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/KdlTreeSolver/KdlTreeSolver.hpp
+libraries/YarpPlugins/KdlTreeSolver/DeviceDriverImpl.cpp
+libraries/YarpPlugins/KdlTreeSolver/ICartesianSolverImpl.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1KdlTreeSolver.png b/classroboticslab_1_1KdlTreeSolver.png
new file mode 100644
index 000000000..2ed17b4ee
Binary files /dev/null and b/classroboticslab_1_1KdlTreeSolver.png differ
diff --git a/classroboticslab_1_1KeyboardController-members.html b/classroboticslab_1_1KeyboardController-members.html
new file mode 100644
index 000000000..638010062
--- /dev/null
+++ b/classroboticslab_1_1KeyboardController-members.html
@@ -0,0 +1,145 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::KeyboardController , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1KeyboardController.html b/classroboticslab_1_1KeyboardController.html
new file mode 100644
index 000000000..3f6a5a42d
--- /dev/null
+++ b/classroboticslab_1_1KeyboardController.html
@@ -0,0 +1,263 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::KeyboardController Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Sends streaming commands to the cartesian controller from a standard keyboard.
+ More...
+
+
#include <KeyboardController.hpp >
+
+
+
+
+
+
+
+ enum joint {
+ Q1 = 0
+, Q2
+, Q3
+, Q4
+,
+ Q5
+, Q6
+, Q7
+, Q8
+,
+ Q9
+, MAX_JOINTS
+
+ }
+
+ enum cart {
+ X = 0
+, Y
+, Z
+, ROTX
+,
+ ROTY
+, ROTZ
+, NUM_CART_COORDS
+
+ }
+
+
+
+
+bool configure (yarp::os::ResourceFinder &rf) override
+
+
+bool updateModule () override
+
+
+bool interruptModule () override
+
+
+double getPeriod () override
+
+
+bool close () override
+
+
+
+ enum control_modes { NOT_CONTROLLING
+, JOINT_MODE
+, CARTESIAN_MODE
+ }
+
+
+
+
+template<typename func >
+void incrementOrDecrementJointVelocity (joint q, func op)
+
+
+template<typename func >
+void incrementOrDecrementCartesianVelocity (cart coord, func op)
+
+
+void toggleReferenceFrame ()
+
+
+void actuateTool (int command)
+
+
+void printJointPositions ()
+
+
+void printCartesianPositions ()
+
+
+void issueStop ()
+
+
+void printHelp ()
+
+
+
+
+static const std::plus< double > increment_functor
+
+
+static const std::minus< double > decrement_functor
+
+
+
+
Uses the terminal as a simple user interface. Also accepts joint commands if connected to a remote controlboard.
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1KeyboardController.png b/classroboticslab_1_1KeyboardController.png
new file mode 100644
index 000000000..349546889
Binary files /dev/null and b/classroboticslab_1_1KeyboardController.png differ
diff --git a/classroboticslab_1_1LeapMotionSensorDevice-members.html b/classroboticslab_1_1LeapMotionSensorDevice-members.html
new file mode 100644
index 000000000..45b35f2bc
--- /dev/null
+++ b/classroboticslab_1_1LeapMotionSensorDevice-members.html
@@ -0,0 +1,118 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::LeapMotionSensorDevice , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1LeapMotionSensorDevice.html b/classroboticslab_1_1LeapMotionSensorDevice.html
new file mode 100644
index 000000000..8a211d9f1
--- /dev/null
+++ b/classroboticslab_1_1LeapMotionSensorDevice.html
@@ -0,0 +1,395 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::LeapMotionSensorDevice Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Represents a LeapMotion device wrapped as an analog sensor by YARP.
+
+
+
#include <LeapMotionSensorDevice.hpp >
+
+
+
+
+
+
+
+
+
+
+
+yarp::dev::IAnalogSensor * iAnalogSensor
+
+
+bool usingPose
+
+
+std::vector< double > initialTcpOffset
+
+
+std::vector< double > initialLeapOffset
+
+
+KDL::Frame frame_base_leap
+
+
+KDL::Frame frame_ee_leap
+
+
+KDL::Frame frame_leap_ee
+
+
+KDL::Frame previousPose
+
+
+double previousTimestamp
+
+
+bool hasActuator
+
+
+bool grab
+
+
+bool pinch
+
+
+
+
+
+ICartesianControl * iCartesianControl
+
+
+std::vector< double > data
+
+
+std::vector< bool > fixedAxes
+
+
+int actuatorState
+
+
+
+
+
◆ acquireData()
+
+
+
+
+
+
+
+
+ bool LeapMotionSensorDevice::acquireData
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ acquireInterfaces()
+
+
+
+
+
+
+
+
+ bool LeapMotionSensorDevice::acquireInterfaces
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ getActuatorState()
+
+
+
+
+
+
+
+
+ int LeapMotionSensorDevice::getActuatorState
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ initialize()
+
+
+
+
+
+
+
+
+ bool LeapMotionSensorDevice::initialize
+ (
+ bool
+ usingStreamingPreset )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ usingStreamingPreset Whether the cartesian controller supports streaming command presets or not.
+
+
+
+
Returns true on success, false otherwise
+
+
Reimplemented from roboticslab::StreamingDevice .
+
+
+
+
+
◆ sendMovementCommand()
+
+
+
+
+
+
+
+
+ void LeapMotionSensorDevice::sendMovementCommand
+ (
+ double
+ timestamp )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ transformData()
+
+
+
+
+
+
+
+
+ bool LeapMotionSensorDevice::transformData
+ (
+ double
+ scaling )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ scaling Scaling factor applied to each data value.
+
+
+
+
Returns true on success, false otherwise
+
+
Reimplemented from roboticslab::StreamingDevice .
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1LeapMotionSensorDevice.png b/classroboticslab_1_1LeapMotionSensorDevice.png
new file mode 100644
index 000000000..be11fbffe
Binary files /dev/null and b/classroboticslab_1_1LeapMotionSensorDevice.png differ
diff --git a/classroboticslab_1_1LinearTrajectoryThread-members.html b/classroboticslab_1_1LinearTrajectoryThread-members.html
new file mode 100644
index 000000000..de8d656b8
--- /dev/null
+++ b/classroboticslab_1_1LinearTrajectoryThread-members.html
@@ -0,0 +1,102 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::LinearTrajectoryThread , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1LinearTrajectoryThread.html b/classroboticslab_1_1LinearTrajectoryThread.html
new file mode 100644
index 000000000..daf1f3069
--- /dev/null
+++ b/classroboticslab_1_1LinearTrajectoryThread.html
@@ -0,0 +1,154 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::LinearTrajectoryThread Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Periodic thread that encapsulates a linear trajectory.
+
+
+
#include <LinearTrajectoryThread.hpp >
+
+
+
+
+
+
+
+
+ LinearTrajectoryThread (int period, ICartesianControl *iCartesianControl)
+
+
+bool checkStreamingConfig ()
+
+
+bool configure (const std::vector< double > &vels)
+
+
+void useTcpFrame (bool enableTcpFrame)
+
+
+
+
+void run () override
+
+
+
+
+double period {0.0}
+
+
+ICartesianControl * iCartesianControl {nullptr}
+
+
+KDL::Trajectory * trajectory {nullptr}
+
+
+double startTime {0.0}
+
+
+bool usingStreamingCommandConfig {false}
+
+
+bool usingTcpFrame {false}
+
+
+std::vector< double > deltaX
+
+
+std::mutex mtx
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1LinearTrajectoryThread.png b/classroboticslab_1_1LinearTrajectoryThread.png
new file mode 100644
index 000000000..cfa12cf29
Binary files /dev/null and b/classroboticslab_1_1LinearTrajectoryThread.png differ
diff --git a/classroboticslab_1_1MatrixExponential-members.html b/classroboticslab_1_1MatrixExponential-members.html
new file mode 100644
index 000000000..e9ffe784a
--- /dev/null
+++ b/classroboticslab_1_1MatrixExponential-members.html
@@ -0,0 +1,101 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::MatrixExponential , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1MatrixExponential.html b/classroboticslab_1_1MatrixExponential.html
new file mode 100644
index 000000000..8fe5d4e86
--- /dev/null
+++ b/classroboticslab_1_1MatrixExponential.html
@@ -0,0 +1,365 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::MatrixExponential Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Abstraction of a term in a product of exponentials (POE) formula.
+ More...
+
+
#include <MatrixExponential.hpp >
+
+
+
+motion motionType
+
+
+KDL::Vector axis
+
+
+KDL::Vector origin
+
+
+
+
+
+
◆ motion
+
+
+
+
+Enumerator ROTATION Revolute joint (zero-pitch twist).
+
+ TRANSLATION Prismatic joint (infinite-pitch twist).
+
+
+
+
+
+
+
+
◆ MatrixExponential()
+
+
+
+
+
+ MatrixExponential::MatrixExponential
+ (
+ motion
+ motionType ,
+
+
+
+
+ const KDL::Vector &
+ axis ,
+
+
+
+
+ const KDL::Vector &
+ origin = KDL::Vector::Zero()
+
+
+
+ )
+
+
+
+
+
Parameters
+
+ motionType Screw motion type as defined in motion .
+ axis Screw axis.
+ origin A point along the screw axis (defaults to (0, 0, 0), ignored in prismatic joints).
+
+
+
+
+
+
+
+
+
◆ asFrame()
+
+
+
+
+
+ KDL::Frame MatrixExponential::asFrame
+ (
+ double
+ theta )
+ const
+
+
+
+
Parameters
+
+ theta Input magnitude this screw should be computed at.
+
+
+
+
Returns Resulting homogeneous transformation matrix.
+
+
+
+
+
◆ changeBase()
+
+
+
+
+
+ void MatrixExponential::changeBase
+ (
+ const KDL::Frame &
+ H_new_old )
+
+
+
+
+
Parameters
+
+ H_new_old Transformation between the new and the old base frame.
+
+
+
+
See also cloneWithBase
+
+
+
+
+
◆ cloneWithBase()
+
+
+
+
+
+ MatrixExponential MatrixExponential::cloneWithBase
+ (
+ const KDL::Frame &
+ H_new_old )
+ const
+
+
+
+
Parameters
+
+ H_new_old Transformation between the new and the old base frame.
+
+
+
+
Returns An instance referred to the new frame.
+
See also changeBase
+
+
+
+
+
◆ getAxis()
+
+
+
+
+
+
+
+
+ const KDL::Vector& roboticslab::MatrixExponential::getAxis
+ (
+ )
+ const
+
+
+
+
+inline
+
+
+
+
Returns A vector representing the direction of the screw axis.
+
+
+
+
+
◆ getMotionType()
+
+
+
+
+
+
+
+
+ motion roboticslab::MatrixExponential::getMotionType
+ (
+ )
+ const
+
+
+
+
+inline
+
+
+
+
Returns A motion type this term has been registered with.
+
+
+
+
+
◆ getOrigin()
+
+
+
+
+
+
+
+
+ const KDL::Vector& roboticslab::MatrixExponential::getOrigin
+ (
+ )
+ const
+
+
+
+
+inline
+
+
+
+
Returns A vector representing the position of some point along the screw axis.
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1PadenKahanOne-members.html b/classroboticslab_1_1PadenKahanOne-members.html
new file mode 100644
index 000000000..8f39c70f5
--- /dev/null
+++ b/classroboticslab_1_1PadenKahanOne-members.html
@@ -0,0 +1,100 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::PadenKahanOne , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1PadenKahanOne.html b/classroboticslab_1_1PadenKahanOne.html
new file mode 100644
index 000000000..a52cfad0b
--- /dev/null
+++ b/classroboticslab_1_1PadenKahanOne.html
@@ -0,0 +1,270 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::PadenKahanOne Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
First Paden-Kahan subproblem.
+ More...
+
+
#include <ScrewTheoryIkSubproblems.hpp >
+
+
+
+
+
+
+
+
+
+
+
+const int id
+
+
+const MatrixExponential exp
+
+
+const KDL::Vector p
+
+
+const KDL::Rotation axisPow
+
+
+
+
Single solution, single revolute joint geometric IK subproblem given by \( e\,^{\hat{\xi}\,{\theta}} \cdot p = k \) (rotation screw applied to a point).
+
+
+
◆ PadenKahanOne()
+
+
+
+
+
+ PadenKahanOne::PadenKahanOne
+ (
+ int
+ id ,
+
+
+
+
+ const MatrixExponential &
+ exp ,
+
+
+
+
+ const KDL::Vector &
+ p
+
+
+
+ )
+
+
+
+
+
Parameters
+
+ id Zero-based joint id of the product of exponentials (POE) term.
+ exp POE term.
+ p Characteristic point.
+
+
+
+
+
+
+
+
+
◆ solve()
+
+
+
+
+
+
+
+
+ bool PadenKahanOne::solve
+ (
+ const KDL::Frame &
+ rhs ,
+
+
+
+
+ const KDL::Frame &
+ pointTransform ,
+
+
+
+
+ Solutions &
+ solutions
+
+
+
+ )
+ const
+
+
+
+
+override virtual
+
+
+
+
Given the product of exponentials (POE) formula \( \prod_i e\,^{\hat{\xi}_i\,{\theta_i}} \cdot H_{ST}(0) = H_{ST}(\theta) \) , , invariant and known terms are rearranged to the right side (rhs
) as follows:
+
+\[ \prod_{i=j}^{j+k} e\,^{\hat{\xi}_i\,{\theta_i}} = \left [ \prod_{i=1}^{j-1} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \cdot H_{ST}(\theta) \cdot \left [ H_{ST}(0) \right ]^{-1} \cdot \left [ \prod_{i=j+k+1}^{N} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \]
+
+
where \( j = \{1, 2, ..., N\}, k = \{1, 2, ..., N-1\}, 1 <= j+k <= N \) .
+
Given \( N \) terms in the POE formula, \( j \) of which are unknowns, any characteristic point \( p \) postmultiplying this expression could be rewritten as \( p' \) per:
+
+\[ \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot \prod_{i=j+1}^N e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p = \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p' \]
+
+
where pointTransform
is the transformation matrix that produces \( p' \) from \( p \) .
+
Parameters
+
+ rhs Right-hand side of the POE formula prior to being applied to the right-hand side of this subproblem.
+ pointTransform Transformation frame applied to the first (and perhaps only) characteristic point of this subproblem.
+ solutions Output vector of local solutions.
+
+
+
+
Returns True if all solutions are reachable, false otherwise.
+
+
Implements roboticslab::ScrewTheoryIkSubproblem .
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1PadenKahanOne.png b/classroboticslab_1_1PadenKahanOne.png
new file mode 100644
index 000000000..62a09e070
Binary files /dev/null and b/classroboticslab_1_1PadenKahanOne.png differ
diff --git a/classroboticslab_1_1PadenKahanThree-members.html b/classroboticslab_1_1PadenKahanThree-members.html
new file mode 100644
index 000000000..e873277c8
--- /dev/null
+++ b/classroboticslab_1_1PadenKahanThree-members.html
@@ -0,0 +1,101 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::PadenKahanThree , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1PadenKahanThree.html b/classroboticslab_1_1PadenKahanThree.html
new file mode 100644
index 000000000..942dacbe7
--- /dev/null
+++ b/classroboticslab_1_1PadenKahanThree.html
@@ -0,0 +1,280 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::PadenKahanThree Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Third Paden-Kahan subproblem.
+ More...
+
+
#include <ScrewTheoryIkSubproblems.hpp >
+
+
+
+
+
+
+
+
+
+
+
+const int id
+
+
+const MatrixExponential exp
+
+
+const KDL::Vector p
+
+
+const KDL::Vector k
+
+
+const KDL::Rotation axisPow
+
+
+
+
Dual solution, single revolute joint geometric IK subproblem given by \( \left \| e\,^{\hat{\xi}\,{\theta}} \cdot p - k \right \| = \delta \) (rotation screw for moving \( p \) to a distance \( \delta \) from \( k \)).
+
+
+
◆ PadenKahanThree()
+
+
+
+
+
+ PadenKahanThree::PadenKahanThree
+ (
+ int
+ id ,
+
+
+
+
+ const MatrixExponential &
+ exp ,
+
+
+
+
+ const KDL::Vector &
+ p ,
+
+
+
+
+ const KDL::Vector &
+ k
+
+
+
+ )
+
+
+
+
+
Parameters
+
+ id Zero-based joint id of the product of exponentials (POE) term.
+ exp POE term.
+ p First characteristic point.
+ k Second characteristic point.
+
+
+
+
+
+
+
+
+
◆ solve()
+
+
+
+
+
+
+
+
+ bool PadenKahanThree::solve
+ (
+ const KDL::Frame &
+ rhs ,
+
+
+
+
+ const KDL::Frame &
+ pointTransform ,
+
+
+
+
+ Solutions &
+ solutions
+
+
+
+ )
+ const
+
+
+
+
+override virtual
+
+
+
+
Given the product of exponentials (POE) formula \( \prod_i e\,^{\hat{\xi}_i\,{\theta_i}} \cdot H_{ST}(0) = H_{ST}(\theta) \) , , invariant and known terms are rearranged to the right side (rhs
) as follows:
+
+\[ \prod_{i=j}^{j+k} e\,^{\hat{\xi}_i\,{\theta_i}} = \left [ \prod_{i=1}^{j-1} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \cdot H_{ST}(\theta) \cdot \left [ H_{ST}(0) \right ]^{-1} \cdot \left [ \prod_{i=j+k+1}^{N} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \]
+
+
where \( j = \{1, 2, ..., N\}, k = \{1, 2, ..., N-1\}, 1 <= j+k <= N \) .
+
Given \( N \) terms in the POE formula, \( j \) of which are unknowns, any characteristic point \( p \) postmultiplying this expression could be rewritten as \( p' \) per:
+
+\[ \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot \prod_{i=j+1}^N e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p = \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p' \]
+
+
where pointTransform
is the transformation matrix that produces \( p' \) from \( p \) .
+
Parameters
+
+ rhs Right-hand side of the POE formula prior to being applied to the right-hand side of this subproblem.
+ pointTransform Transformation frame applied to the first (and perhaps only) characteristic point of this subproblem.
+ solutions Output vector of local solutions.
+
+
+
+
Returns True if all solutions are reachable, false otherwise.
+
+
Implements roboticslab::ScrewTheoryIkSubproblem .
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1PadenKahanThree.png b/classroboticslab_1_1PadenKahanThree.png
new file mode 100644
index 000000000..91452091f
Binary files /dev/null and b/classroboticslab_1_1PadenKahanThree.png differ
diff --git a/classroboticslab_1_1PadenKahanTwo-members.html b/classroboticslab_1_1PadenKahanTwo-members.html
new file mode 100644
index 000000000..4d4729828
--- /dev/null
+++ b/classroboticslab_1_1PadenKahanTwo-members.html
@@ -0,0 +1,106 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::PadenKahanTwo , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1PadenKahanTwo.html b/classroboticslab_1_1PadenKahanTwo.html
new file mode 100644
index 000000000..feedae523
--- /dev/null
+++ b/classroboticslab_1_1PadenKahanTwo.html
@@ -0,0 +1,309 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::PadenKahanTwo Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Second Paden-Kahan subproblem.
+ More...
+
+
#include <ScrewTheoryIkSubproblems.hpp >
+
+
+
+
+
+
+
+
+
+
+
+const int id1
+
+
+const int id2
+
+
+const MatrixExponential exp1
+
+
+const MatrixExponential exp2
+
+
+const KDL::Vector p
+
+
+const KDL::Vector r
+
+
+const KDL::Vector axesCross
+
+
+const KDL::Rotation axisPow1
+
+
+const KDL::Rotation axisPow2
+
+
+const double axesDot
+
+
+
+
Dual solution, double revolute joint geometric IK subproblem given by \( e\,^{\hat{\xi_1}\,{\theta_1}} \cdot e\,^{\hat{\xi_2}\,{\theta_2}} \cdot p = k \) (consecutive crossing rotation screws to a point).
+
+
+
◆ PadenKahanTwo()
+
+
+
+
+
+ PadenKahanTwo::PadenKahanTwo
+ (
+ int
+ id1 ,
+
+
+
+
+ int
+ id2 ,
+
+
+
+
+ const MatrixExponential &
+ exp1 ,
+
+
+
+
+ const MatrixExponential &
+ exp2 ,
+
+
+
+
+ const KDL::Vector &
+ p ,
+
+
+
+
+ const KDL::Vector &
+ r
+
+
+
+ )
+
+
+
+
+
Parameters
+
+ id1 Zero-based joint id of the first product of exponentials (POE) term.
+ id2 Zero-based joint id of the second POE term.
+ exp1 First POE term.
+ exp2 Second POE term.
+ p Characteristic point.
+ r Point of intersection between both screw axes.
+
+
+
+
+
+
+
+
+
◆ solve()
+
+
+
+
+
+
+
+
+ bool PadenKahanTwo::solve
+ (
+ const KDL::Frame &
+ rhs ,
+
+
+
+
+ const KDL::Frame &
+ pointTransform ,
+
+
+
+
+ Solutions &
+ solutions
+
+
+
+ )
+ const
+
+
+
+
+override virtual
+
+
+
+
Given the product of exponentials (POE) formula \( \prod_i e\,^{\hat{\xi}_i\,{\theta_i}} \cdot H_{ST}(0) = H_{ST}(\theta) \) , , invariant and known terms are rearranged to the right side (rhs
) as follows:
+
+\[ \prod_{i=j}^{j+k} e\,^{\hat{\xi}_i\,{\theta_i}} = \left [ \prod_{i=1}^{j-1} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \cdot H_{ST}(\theta) \cdot \left [ H_{ST}(0) \right ]^{-1} \cdot \left [ \prod_{i=j+k+1}^{N} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \]
+
+
where \( j = \{1, 2, ..., N\}, k = \{1, 2, ..., N-1\}, 1 <= j+k <= N \) .
+
Given \( N \) terms in the POE formula, \( j \) of which are unknowns, any characteristic point \( p \) postmultiplying this expression could be rewritten as \( p' \) per:
+
+\[ \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot \prod_{i=j+1}^N e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p = \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p' \]
+
+
where pointTransform
is the transformation matrix that produces \( p' \) from \( p \) .
+
Parameters
+
+ rhs Right-hand side of the POE formula prior to being applied to the right-hand side of this subproblem.
+ pointTransform Transformation frame applied to the first (and perhaps only) characteristic point of this subproblem.
+ solutions Output vector of local solutions.
+
+
+
+
Returns True if all solutions are reachable, false otherwise.
+
+
Implements roboticslab::ScrewTheoryIkSubproblem .
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1PadenKahanTwo.png b/classroboticslab_1_1PadenKahanTwo.png
new file mode 100644
index 000000000..1bee8d1a7
Binary files /dev/null and b/classroboticslab_1_1PadenKahanTwo.png differ
diff --git a/classroboticslab_1_1PardosGotorFour-members.html b/classroboticslab_1_1PardosGotorFour-members.html
new file mode 100644
index 000000000..91bcaa406
--- /dev/null
+++ b/classroboticslab_1_1PardosGotorFour-members.html
@@ -0,0 +1,103 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::PardosGotorFour , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1PardosGotorFour.html b/classroboticslab_1_1PardosGotorFour.html
new file mode 100644
index 000000000..4ada77388
--- /dev/null
+++ b/classroboticslab_1_1PardosGotorFour.html
@@ -0,0 +1,293 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::PardosGotorFour Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Fourth Pardos-Gotor subproblem.
+ More...
+
+
#include <ScrewTheoryIkSubproblems.hpp >
+
+
+
+
+
+
+
+
+
+
+
+const int id1
+
+
+const int id2
+
+
+const MatrixExponential exp1
+
+
+const MatrixExponential exp2
+
+
+const KDL::Vector p
+
+
+const KDL::Vector n
+
+
+const KDL::Rotation axisPow
+
+
+
+
Dual solution, double revolute joint geometric IK subproblem given by \( e\,^{\hat{\xi_1}\,{\theta_1}} \cdot e\,^{\hat{\xi_2}\,{\theta_2}} \cdot p = k \) (two consecutive parallel rotation screws applied to a point, see [4] ).
+
+
+
◆ PardosGotorFour()
+
+
+
+
+
+ PardosGotorFour::PardosGotorFour
+ (
+ int
+ id1 ,
+
+
+
+
+ int
+ id2 ,
+
+
+
+
+ const MatrixExponential &
+ exp1 ,
+
+
+
+
+ const MatrixExponential &
+ exp2 ,
+
+
+
+
+ const KDL::Vector &
+ p
+
+
+
+ )
+
+
+
+
+
Parameters
+
+ id1 Zero-based joint id of the first product of exponentials (POE) term.
+ id2 Zero-based joint id of the second POE term.
+ exp1 First POE term.
+ exp2 Second POE term.
+ p Characteristic point.
+
+
+
+
+
+
+
+
+
◆ solve()
+
+
+
+
+
+
+
+
+ bool PardosGotorFour::solve
+ (
+ const KDL::Frame &
+ rhs ,
+
+
+
+
+ const KDL::Frame &
+ pointTransform ,
+
+
+
+
+ Solutions &
+ solutions
+
+
+
+ )
+ const
+
+
+
+
+override virtual
+
+
+
+
Given the product of exponentials (POE) formula \( \prod_i e\,^{\hat{\xi}_i\,{\theta_i}} \cdot H_{ST}(0) = H_{ST}(\theta) \) , , invariant and known terms are rearranged to the right side (rhs
) as follows:
+
+\[ \prod_{i=j}^{j+k} e\,^{\hat{\xi}_i\,{\theta_i}} = \left [ \prod_{i=1}^{j-1} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \cdot H_{ST}(\theta) \cdot \left [ H_{ST}(0) \right ]^{-1} \cdot \left [ \prod_{i=j+k+1}^{N} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \]
+
+
where \( j = \{1, 2, ..., N\}, k = \{1, 2, ..., N-1\}, 1 <= j+k <= N \) .
+
Given \( N \) terms in the POE formula, \( j \) of which are unknowns, any characteristic point \( p \) postmultiplying this expression could be rewritten as \( p' \) per:
+
+\[ \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot \prod_{i=j+1}^N e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p = \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p' \]
+
+
where pointTransform
is the transformation matrix that produces \( p' \) from \( p \) .
+
Parameters
+
+ rhs Right-hand side of the POE formula prior to being applied to the right-hand side of this subproblem.
+ pointTransform Transformation frame applied to the first (and perhaps only) characteristic point of this subproblem.
+ solutions Output vector of local solutions.
+
+
+
+
Returns True if all solutions are reachable, false otherwise.
+
+
Implements roboticslab::ScrewTheoryIkSubproblem .
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1PardosGotorFour.png b/classroboticslab_1_1PardosGotorFour.png
new file mode 100644
index 000000000..da9e6e44b
Binary files /dev/null and b/classroboticslab_1_1PardosGotorFour.png differ
diff --git a/classroboticslab_1_1PardosGotorOne-members.html b/classroboticslab_1_1PardosGotorOne-members.html
new file mode 100644
index 000000000..902c792bb
--- /dev/null
+++ b/classroboticslab_1_1PardosGotorOne-members.html
@@ -0,0 +1,99 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::PardosGotorOne , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1PardosGotorOne.html b/classroboticslab_1_1PardosGotorOne.html
new file mode 100644
index 000000000..61b6419c7
--- /dev/null
+++ b/classroboticslab_1_1PardosGotorOne.html
@@ -0,0 +1,267 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::PardosGotorOne Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
First Pardos-Gotor subproblem.
+ More...
+
+
#include <ScrewTheoryIkSubproblems.hpp >
+
+
+
+
+
+
+
+
+
+
+
Single solution, single prismatic joint geometric IK subproblem given by \( e\,^{\hat{\xi}\,{\theta}} \cdot p = k \) (translation screw applied to a point, see [4] ).
+
+
+
◆ PardosGotorOne()
+
+
+
+
+
+ PardosGotorOne::PardosGotorOne
+ (
+ int
+ id ,
+
+
+
+
+ const MatrixExponential &
+ exp ,
+
+
+
+
+ const KDL::Vector &
+ p
+
+
+
+ )
+
+
+
+
+
Parameters
+
+ id Zero-based joint id of the product of exponentials (POE) term.
+ exp POE term.
+ p Characteristic point.
+
+
+
+
+
+
+
+
+
◆ solve()
+
+
+
+
+
+
+
+
+ bool PardosGotorOne::solve
+ (
+ const KDL::Frame &
+ rhs ,
+
+
+
+
+ const KDL::Frame &
+ pointTransform ,
+
+
+
+
+ Solutions &
+ solutions
+
+
+
+ )
+ const
+
+
+
+
+override virtual
+
+
+
+
Given the product of exponentials (POE) formula \( \prod_i e\,^{\hat{\xi}_i\,{\theta_i}} \cdot H_{ST}(0) = H_{ST}(\theta) \) , , invariant and known terms are rearranged to the right side (rhs
) as follows:
+
+\[ \prod_{i=j}^{j+k} e\,^{\hat{\xi}_i\,{\theta_i}} = \left [ \prod_{i=1}^{j-1} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \cdot H_{ST}(\theta) \cdot \left [ H_{ST}(0) \right ]^{-1} \cdot \left [ \prod_{i=j+k+1}^{N} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \]
+
+
where \( j = \{1, 2, ..., N\}, k = \{1, 2, ..., N-1\}, 1 <= j+k <= N \) .
+
Given \( N \) terms in the POE formula, \( j \) of which are unknowns, any characteristic point \( p \) postmultiplying this expression could be rewritten as \( p' \) per:
+
+\[ \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot \prod_{i=j+1}^N e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p = \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p' \]
+
+
where pointTransform
is the transformation matrix that produces \( p' \) from \( p \) .
+
Parameters
+
+ rhs Right-hand side of the POE formula prior to being applied to the right-hand side of this subproblem.
+ pointTransform Transformation frame applied to the first (and perhaps only) characteristic point of this subproblem.
+ solutions Output vector of local solutions.
+
+
+
+
Returns True if all solutions are reachable, false otherwise.
+
+
Implements roboticslab::ScrewTheoryIkSubproblem .
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1PardosGotorOne.png b/classroboticslab_1_1PardosGotorOne.png
new file mode 100644
index 000000000..2c6d25bd5
Binary files /dev/null and b/classroboticslab_1_1PardosGotorOne.png differ
diff --git a/classroboticslab_1_1PardosGotorThree-members.html b/classroboticslab_1_1PardosGotorThree-members.html
new file mode 100644
index 000000000..83788938c
--- /dev/null
+++ b/classroboticslab_1_1PardosGotorThree-members.html
@@ -0,0 +1,100 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::PardosGotorThree , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1PardosGotorThree.html b/classroboticslab_1_1PardosGotorThree.html
new file mode 100644
index 000000000..4dd5920bb
--- /dev/null
+++ b/classroboticslab_1_1PardosGotorThree.html
@@ -0,0 +1,277 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::PardosGotorThree Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Third Pardos-Gotor subproblem.
+ More...
+
+
#include <ScrewTheoryIkSubproblems.hpp >
+
+
+
+
+
+
+
+
+
+
+
+const int id
+
+
+const MatrixExponential exp
+
+
+const KDL::Vector p
+
+
+const KDL::Vector k
+
+
+
+
Dual solution, single prismatic joint geometric IK subproblem given by \( \left \| e\,^{\hat{\xi}\,{\theta}} \cdot p - k \right \| = \delta \) (translation screw for moving \( p \) to a distance \( \delta \) from \( k \), see [4] ).
+
+
+
◆ PardosGotorThree()
+
+
+
+
+
+ PardosGotorThree::PardosGotorThree
+ (
+ int
+ id ,
+
+
+
+
+ const MatrixExponential &
+ exp ,
+
+
+
+
+ const KDL::Vector &
+ p ,
+
+
+
+
+ const KDL::Vector &
+ k
+
+
+
+ )
+
+
+
+
+
Parameters
+
+ id Zero-based joint id of the product of exponentials (POE) term.
+ exp POE term.
+ p First characteristic point.
+ k Second characteristic point.
+
+
+
+
+
+
+
+
+
◆ solve()
+
+
+
+
+
+
+
+
+ bool PardosGotorThree::solve
+ (
+ const KDL::Frame &
+ rhs ,
+
+
+
+
+ const KDL::Frame &
+ pointTransform ,
+
+
+
+
+ Solutions &
+ solutions
+
+
+
+ )
+ const
+
+
+
+
+override virtual
+
+
+
+
Given the product of exponentials (POE) formula \( \prod_i e\,^{\hat{\xi}_i\,{\theta_i}} \cdot H_{ST}(0) = H_{ST}(\theta) \) , , invariant and known terms are rearranged to the right side (rhs
) as follows:
+
+\[ \prod_{i=j}^{j+k} e\,^{\hat{\xi}_i\,{\theta_i}} = \left [ \prod_{i=1}^{j-1} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \cdot H_{ST}(\theta) \cdot \left [ H_{ST}(0) \right ]^{-1} \cdot \left [ \prod_{i=j+k+1}^{N} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \]
+
+
where \( j = \{1, 2, ..., N\}, k = \{1, 2, ..., N-1\}, 1 <= j+k <= N \) .
+
Given \( N \) terms in the POE formula, \( j \) of which are unknowns, any characteristic point \( p \) postmultiplying this expression could be rewritten as \( p' \) per:
+
+\[ \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot \prod_{i=j+1}^N e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p = \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p' \]
+
+
where pointTransform
is the transformation matrix that produces \( p' \) from \( p \) .
+
Parameters
+
+ rhs Right-hand side of the POE formula prior to being applied to the right-hand side of this subproblem.
+ pointTransform Transformation frame applied to the first (and perhaps only) characteristic point of this subproblem.
+ solutions Output vector of local solutions.
+
+
+
+
Returns True if all solutions are reachable, false otherwise.
+
+
Implements roboticslab::ScrewTheoryIkSubproblem .
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1PardosGotorThree.png b/classroboticslab_1_1PardosGotorThree.png
new file mode 100644
index 000000000..3e9a065b5
Binary files /dev/null and b/classroboticslab_1_1PardosGotorThree.png differ
diff --git a/classroboticslab_1_1PardosGotorTwo-members.html b/classroboticslab_1_1PardosGotorTwo-members.html
new file mode 100644
index 000000000..968777439
--- /dev/null
+++ b/classroboticslab_1_1PardosGotorTwo-members.html
@@ -0,0 +1,103 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::PardosGotorTwo , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1PardosGotorTwo.html b/classroboticslab_1_1PardosGotorTwo.html
new file mode 100644
index 000000000..248fdc489
--- /dev/null
+++ b/classroboticslab_1_1PardosGotorTwo.html
@@ -0,0 +1,293 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::PardosGotorTwo Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Second Pardos-Gotor subproblem.
+ More...
+
+
#include <ScrewTheoryIkSubproblems.hpp >
+
+
+
+
+
+
+
+
+
+
+
+const int id1
+
+
+const int id2
+
+
+const MatrixExponential exp1
+
+
+const MatrixExponential exp2
+
+
+const KDL::Vector p
+
+
+const KDL::Vector crossPr2
+
+
+const double crossPr2Norm
+
+
+
+
Single solution, double prismatic joint geometric IK subproblem given by \( e\,^{\hat{\xi_1}\,{\theta_1}} \cdot e\,^{\hat{\xi_2}\,{\theta_2}} \cdot p = k \) (consecutive translation screws to a point, see [4] ).
+
+
+
◆ PardosGotorTwo()
+
+
+
+
+
+ PardosGotorTwo::PardosGotorTwo
+ (
+ int
+ id1 ,
+
+
+
+
+ int
+ id2 ,
+
+
+
+
+ const MatrixExponential &
+ exp1 ,
+
+
+
+
+ const MatrixExponential &
+ exp2 ,
+
+
+
+
+ const KDL::Vector &
+ p
+
+
+
+ )
+
+
+
+
+
Parameters
+
+ id1 Zero-based joint id of the first product of exponentials (POE) term.
+ id2 Zero-based joint id of the second POE term.
+ exp1 First POE term.
+ exp2 Second POE term.
+ p Characteristic point.
+
+
+
+
+
+
+
+
+
◆ solve()
+
+
+
+
+
+
+
+
+ bool PardosGotorTwo::solve
+ (
+ const KDL::Frame &
+ rhs ,
+
+
+
+
+ const KDL::Frame &
+ pointTransform ,
+
+
+
+
+ Solutions &
+ solutions
+
+
+
+ )
+ const
+
+
+
+
+override virtual
+
+
+
+
Given the product of exponentials (POE) formula \( \prod_i e\,^{\hat{\xi}_i\,{\theta_i}} \cdot H_{ST}(0) = H_{ST}(\theta) \) , , invariant and known terms are rearranged to the right side (rhs
) as follows:
+
+\[ \prod_{i=j}^{j+k} e\,^{\hat{\xi}_i\,{\theta_i}} = \left [ \prod_{i=1}^{j-1} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \cdot H_{ST}(\theta) \cdot \left [ H_{ST}(0) \right ]^{-1} \cdot \left [ \prod_{i=j+k+1}^{N} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \]
+
+
where \( j = \{1, 2, ..., N\}, k = \{1, 2, ..., N-1\}, 1 <= j+k <= N \) .
+
Given \( N \) terms in the POE formula, \( j \) of which are unknowns, any characteristic point \( p \) postmultiplying this expression could be rewritten as \( p' \) per:
+
+\[ \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot \prod_{i=j+1}^N e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p = \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p' \]
+
+
where pointTransform
is the transformation matrix that produces \( p' \) from \( p \) .
+
Parameters
+
+ rhs Right-hand side of the POE formula prior to being applied to the right-hand side of this subproblem.
+ pointTransform Transformation frame applied to the first (and perhaps only) characteristic point of this subproblem.
+ solutions Output vector of local solutions.
+
+
+
+
Returns True if all solutions are reachable, false otherwise.
+
+
Implements roboticslab::ScrewTheoryIkSubproblem .
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1PardosGotorTwo.png b/classroboticslab_1_1PardosGotorTwo.png
new file mode 100644
index 000000000..da2a72fa7
Binary files /dev/null and b/classroboticslab_1_1PardosGotorTwo.png differ
diff --git a/classroboticslab_1_1PoeExpression-members.html b/classroboticslab_1_1PoeExpression-members.html
new file mode 100644
index 000000000..2b8f62184
--- /dev/null
+++ b/classroboticslab_1_1PoeExpression-members.html
@@ -0,0 +1,103 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::PoeExpression , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1PoeExpression.html b/classroboticslab_1_1PoeExpression.html
new file mode 100644
index 000000000..2ab96ed11
--- /dev/null
+++ b/classroboticslab_1_1PoeExpression.html
@@ -0,0 +1,541 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::PoeExpression Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Abstraction of a product of exponentials (POE) formula.
+ More...
+
+
#include <ProductOfExponentials.hpp >
+
+
+
This entity is comprised of a sequence of matrix exponentials and a transformation between the base and the tool frame.
+
See also MatrixExponential
+
+
+
◆ PoeExpression()
+
+
+
+
+
+
+
+
+ roboticslab::PoeExpression::PoeExpression
+ (
+ const KDL::Frame &
+ H_S_T = KDL::Frame::Identity()
)
+
+
+
+
+
+inline explicit
+
+
+
+
Parameters
+
+ H_S_T Transformation between the base and the tool frame.
+
+
+
+
+
+
+
+
+
◆ append() [1/2]
+
+
+
+
+
+
+
+
+ void roboticslab::PoeExpression::append
+ (
+ const MatrixExponential &
+ exp ,
+
+
+
+
+ const KDL::Frame &
+ H_new_old = KDL::Frame::Identity()
+
+
+
+ )
+
+
+
+
+
+inline
+
+
+
+
All internal coordinates of the input POE term are referred to the current base frame after applying an (optional) intermediate transformation.
+
Parameters
+
+ exp Input POE term.
+ H_new_old Transformation between the base frame of this POE and the base frame of the input POE term.
+
+
+
+
+
+
+
+
◆ append() [2/2]
+
+
+
+
+
+ void PoeExpression::append
+ (
+ const PoeExpression &
+ poe ,
+
+
+
+
+ const KDL::Frame &
+ H_new_old = KDL::Frame::Identity()
+
+
+
+ )
+
+
+
+
+
All internal coordinates of the input POE are referred to the current base frame after applying an (optional) intermediate transformation. The current tool frame is replaced by the appended POE's tool frame and updated in the same manner.
+
Parameters
+
+ poe Input POE formula.
+ H_new_old Transformation between the new and the old base frame.
+
+
+
+
+
+
+
+
◆ changeBaseFrame()
+
+
+
+
+
+ void PoeExpression::changeBaseFrame
+ (
+ const KDL::Frame &
+ H_new_old )
+
+
+
+
+
All POE terms as well as the base-to-tool reference frame will be updated accordingly.
+
Parameters
+
+ H_new_old Transformation between the new and the old base frame.
+
+
+
+
+
+
+
+
◆ changeToolFrame()
+
+
+
+
+
+
+
+
+ void roboticslab::PoeExpression::changeToolFrame
+ (
+ const KDL::Frame &
+ H_new_old )
+
+
+
+
+
+inline
+
+
+
+
Parameters
+
+ H_new_old Transformation between the new and the old tool frame.
+
+
+
+
+
+
+
+
◆ evaluate()
+
+
+
+
+
+ bool PoeExpression::evaluate
+ (
+ const KDL::JntArray &
+ q ,
+
+
+
+
+ KDL::Frame &
+ H
+
+
+
+ )
+ const
+
+
+
+
Parameters
+
+ q Input joint array (radians).
+ H Output pose in cartesian space.
+
+
+
+
Returns False if the size of the input joint array does not match the size of this POE.
+
+
+
+
+
◆ exponentialAtJoint()
+
+
+
+
+
+
+
+
+ const MatrixExponential & roboticslab::PoeExpression::exponentialAtJoint
+ (
+ int
+ i )
+ const
+
+
+
+
+inline
+
+
+
+
Parameters
+
+ i Zero-based index of the requested term.
+
+
+
+
Returns An unmodifiable reference to said term.
+
+
+
+
+
◆ fromChain()
+
+
+
+
+
+
+
+
+ PoeExpression PoeExpression::fromChain
+ (
+ const KDL::Chain &
+ chain )
+
+
+
+
+
+static
+
+
+
+
Parameters
+
+ chain A KDL kinematic chain representation.
+
+
+
+
Returns The resulting POE formula.
+
See also toChain
+
+
+
+
+
◆ getTransform()
+
+
+
+
+
+
+
+
+ const KDL::Frame& roboticslab::PoeExpression::getTransform
+ (
+ )
+ const
+
+
+
+
+inline
+
+
+
+
Returns Transformation between the base and the tool frame.
+
+
+
+
+
◆ makeReverse()
+
+
+
+
The sequence of POE terms is reversed and the base and tool frames interchanged. Note that theta_i = -theta_(size-i)_reversed.
+
Returns A reversed copy of this instance.
+
See also reverseSelf
+
+
+
+
+
◆ reverseSelf()
+
+
+
+
+
+ void PoeExpression::reverseSelf
+ (
+ )
+
+
+
+
+
The sequence of POE terms is reversed and the base and tool frames interchanged. Note that theta_i = -theta_(size-i)_reversed.
+
See also makeReverse
+
+
+
+
+
◆ size()
+
+
+
+
+
+
+
+
+ int roboticslab::PoeExpression::size
+ (
+ )
+ const
+
+
+
+
+inline
+
+
+
+
Returns Number of terms in this POE formula.
+
+
+
+
+
◆ toChain()
+
+
+
+
+
+ KDL::Chain PoeExpression::toChain
+ (
+ )
+ const
+
+
+
+
Returns A KDL kinematic chain representation.
+
See also fromChain
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1RpcResponder-members.html b/classroboticslab_1_1RpcResponder-members.html
new file mode 100644
index 000000000..c2c909455
--- /dev/null
+++ b/classroboticslab_1_1RpcResponder-members.html
@@ -0,0 +1,107 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::RpcResponder , including all inherited members.
+
+ ConsumerFun typedef (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ FunctionFun typedef (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleActMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleConsumerCmdMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out, ConsumerFun cmd) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleFunctionCmdMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out, FunctionFun cmd) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleParameterGetter (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleParameterGetterGroup (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleParameterSetter (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleParameterSetterGroup (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleRunnableCmdMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out, RunnableFun cmd) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleStatMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleWaitMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ iCartesianControl (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ makeUsage ()roboticslab::RpcResponder
+ respond (const yarp::os::Bottle &in, yarp::os::Bottle &out) overrideroboticslab::RpcResponder
+ RpcResponder (roboticslab::ICartesianControl *_iCartesianControl) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder inline
+ RunnableFun typedef (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ transformIncomingData (std::vector< double > &vin) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder inline protected virtual
+ transformOutgoingData (std::vector< double > &vout) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder inline protected virtual
+
+
+
+
+
diff --git a/classroboticslab_1_1RpcResponder.html b/classroboticslab_1_1RpcResponder.html
new file mode 100644
index 000000000..c229a5dd8
--- /dev/null
+++ b/classroboticslab_1_1RpcResponder.html
@@ -0,0 +1,244 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::RpcResponder Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Responds to RPC command messages.
+
+
+
#include <CartesianControlServer.hpp >
+
+
+
+
+
+
+
+
+
+
+
+virtual bool transformIncomingData (std::vector< double > &vin)
+
+
+virtual bool transformOutgoingData (std::vector< double > &vout)
+
+
+
+
+using RunnableFun = bool(ICartesianControl::*)()
+
+
+using ConsumerFun = bool(ICartesianControl::*)(const std::vector< double > &)
+
+
+using FunctionFun = bool(ICartesianControl::*)(const std::vector< double > &, std::vector< double > &)
+
+
+
+
+bool handleStatMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out)
+
+
+bool handleWaitMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out)
+
+
+bool handleActMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out)
+
+
+bool handleRunnableCmdMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out, RunnableFun cmd)
+
+
+bool handleConsumerCmdMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out, ConsumerFun cmd)
+
+
+bool handleFunctionCmdMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out, FunctionFun cmd)
+
+
+bool handleParameterSetter (const yarp::os::Bottle &in, yarp::os::Bottle &out)
+
+
+bool handleParameterGetter (const yarp::os::Bottle &in, yarp::os::Bottle &out)
+
+
+bool handleParameterSetterGroup (const yarp::os::Bottle &in, yarp::os::Bottle &out)
+
+
+bool handleParameterGetterGroup (const yarp::os::Bottle &in, yarp::os::Bottle &out)
+
+
+
+
+
◆ makeUsage()
+
+
+
+
+
+ void RpcResponder::makeUsage
+ (
+ )
+
+
+
+
+
Generate command usage information.
+
+
+
+
+
◆ respond()
+
+
+
+
+
+
+
+
+ bool RpcResponder::respond
+ (
+ const yarp::os::Bottle &
+ in ,
+
+
+
+
+ yarp::os::Bottle &
+ out
+
+
+
+ )
+
+
+
+
+
+override
+
+
+
+
Respond to a message.
Parameters
+
+ in the message
+ out the response
+
+
+
+
Returns true if there was no critical failure
+
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp
+libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1RpcResponder.png b/classroboticslab_1_1RpcResponder.png
new file mode 100644
index 000000000..d4873a723
Binary files /dev/null and b/classroboticslab_1_1RpcResponder.png differ
diff --git a/classroboticslab_1_1RpcTransformResponder-members.html b/classroboticslab_1_1RpcTransformResponder-members.html
new file mode 100644
index 000000000..40a8e211b
--- /dev/null
+++ b/classroboticslab_1_1RpcTransformResponder-members.html
@@ -0,0 +1,111 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::RpcTransformResponder , including all inherited members.
+
+ ConsumerFun typedef (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ coord (defined in roboticslab::RpcTransformResponder )roboticslab::RpcTransformResponder private
+ FunctionFun typedef (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleActMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleConsumerCmdMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out, ConsumerFun cmd) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleFunctionCmdMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out, FunctionFun cmd) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleParameterGetter (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleParameterGetterGroup (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleParameterSetter (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleParameterSetterGroup (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleRunnableCmdMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out, RunnableFun cmd) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleStatMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ handleWaitMsg (const yarp::os::Bottle &in, yarp::os::Bottle &out) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ iCartesianControl (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ makeUsage ()roboticslab::RpcResponder
+ orient (defined in roboticslab::RpcTransformResponder )roboticslab::RpcTransformResponder private
+ respond (const yarp::os::Bottle &in, yarp::os::Bottle &out) overrideroboticslab::RpcResponder
+ RpcResponder (roboticslab::ICartesianControl *_iCartesianControl) (defined in roboticslab::RpcResponder )roboticslab::RpcResponder inline
+ RpcTransformResponder (roboticslab::ICartesianControl *iCartesianControl, KinRepresentation::coordinate_system coord, KinRepresentation::orientation_system orient, KinRepresentation::angular_units units) (defined in roboticslab::RpcTransformResponder )roboticslab::RpcTransformResponder inline
+ RunnableFun typedef (defined in roboticslab::RpcResponder )roboticslab::RpcResponder private
+ transformIncomingData (std::vector< double > &vin) override (defined in roboticslab::RpcTransformResponder )roboticslab::RpcTransformResponder private virtual
+ transformOutgoingData (std::vector< double > &vout) override (defined in roboticslab::RpcTransformResponder )roboticslab::RpcTransformResponder private virtual
+ units (defined in roboticslab::RpcTransformResponder )roboticslab::RpcTransformResponder private
+
+
+
+
+
diff --git a/classroboticslab_1_1RpcTransformResponder.html b/classroboticslab_1_1RpcTransformResponder.html
new file mode 100644
index 000000000..ed6547ce1
--- /dev/null
+++ b/classroboticslab_1_1RpcTransformResponder.html
@@ -0,0 +1,147 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::RpcTransformResponder Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Responds to RPC command messages, transforms incoming data.
+
+
+
#include <CartesianControlServer.hpp >
+
+
+
+
+
+
+
+
+
+
+
+bool transformIncomingData (std::vector< double > &vin) override
+
+
+bool transformOutgoingData (std::vector< double > &vout) override
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp
+libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1RpcTransformResponder.png b/classroboticslab_1_1RpcTransformResponder.png
new file mode 100644
index 000000000..c432600a9
Binary files /dev/null and b/classroboticslab_1_1RpcTransformResponder.png differ
diff --git a/classroboticslab_1_1ScrewTheoryIkProblem-members.html b/classroboticslab_1_1ScrewTheoryIkProblem-members.html
new file mode 100644
index 000000000..cc1a160d6
--- /dev/null
+++ b/classroboticslab_1_1ScrewTheoryIkProblem-members.html
@@ -0,0 +1,112 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ScrewTheoryIkProblem , including all inherited members.
+
+ create (const PoeExpression &poe, const Steps &steps, bool reversed=false)roboticslab::ScrewTheoryIkProblem static
+ EXP_COMPUTED enum value (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ EXP_KNOWN enum value (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ EXP_UNKNOWN enum value (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ Frames typedef (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ getSteps () constroboticslab::ScrewTheoryIkProblem inline
+ isReversed () constroboticslab::ScrewTheoryIkProblem inline
+ operator= (const ScrewTheoryIkProblem &)=delete (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem
+ poe (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ poe_term enum name (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ PoeTerms typedef (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ recalculateFrames (const Solutions &solutions, Frames &frames, PoeTerms &poeTerms) (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ recalculateFrames (const Solutions &solutions, Frames &frames, PoeTerms &poeTerms, bool backwards) (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ reversed (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ ScrewTheoryIkProblem (const ScrewTheoryIkProblem &)=delete (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem
+ ScrewTheoryIkProblem (const PoeExpression &poe, const Steps &steps, bool reversed) (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ soln (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ Solutions typedefroboticslab::ScrewTheoryIkProblem
+ solutions () constroboticslab::ScrewTheoryIkProblem inline
+ solve (const KDL::Frame &H_S_T, Solutions &solutions)roboticslab::ScrewTheoryIkProblem
+ Steps typedefroboticslab::ScrewTheoryIkProblem
+ steps (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ transformPoint (const KDL::JntArray &jointValues, const PoeTerms &poeTerms) (defined in roboticslab::ScrewTheoryIkProblem )roboticslab::ScrewTheoryIkProblem private
+ ~ScrewTheoryIkProblem ()roboticslab::ScrewTheoryIkProblem
+
+
+
+
+
diff --git a/classroboticslab_1_1ScrewTheoryIkProblem.html b/classroboticslab_1_1ScrewTheoryIkProblem.html
new file mode 100644
index 000000000..d1c68aa62
--- /dev/null
+++ b/classroboticslab_1_1ScrewTheoryIkProblem.html
@@ -0,0 +1,283 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ScrewTheoryIkProblem Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Proxy IK problem solver class that iterates over a sequence of subproblems.
+ More...
+
+
#include <ScrewTheoryIkProblem.hpp >
+
+
+
+using Steps = std::vector< const ScrewTheoryIkSubproblem * >
+ Ordered sequence of IK subproblems needed to solve a IK problem.
+
+
+using Solutions = std::vector< KDL::JntArray >
+ Collection of global IK solutions.
+
+
+
+ enum poe_term { EXP_KNOWN
+, EXP_COMPUTED
+, EXP_UNKNOWN
+ }
+
+
+using Frames = std::vector< KDL::Frame >
+
+
+using PoeTerms = std::vector< poe_term >
+
+
+
+
+ ScrewTheoryIkProblem (const PoeExpression &poe, const Steps &steps, bool reversed)
+
+
+void recalculateFrames (const Solutions &solutions , Frames &frames, PoeTerms &poeTerms)
+
+
+bool recalculateFrames (const Solutions &solutions , Frames &frames, PoeTerms &poeTerms, bool backwards)
+
+
+KDL::Frame transformPoint (const KDL::JntArray &jointValues, const PoeTerms &poeTerms)
+
+
+
+
+const PoeExpression poe
+
+
+const Steps steps
+
+
+const bool reversed
+
+
+const int soln
+
+
+
+
This class is immutable. Instantiation is allowed by means of a static builder method.
+
See also ScrewTheoryIkProblemBuilder
+
+
+
◆ create()
+
+
+
+
Parameters
+
+ poe A product of exponentials (POE) formula.
+ steps Collection of subproblems that solve this particular IK problem.
+ reversed True if the POE has been reversed (in order to find a valid solution).
+
+
+
+
Returns An instance of an IK problem solver.
+
+
+
+
+
◆ solve()
+
+
+
+
+
+ bool ScrewTheoryIkProblem::solve
+ (
+ const KDL::Frame &
+ H_S_T ,
+
+
+
+
+ Solutions &
+ solutions
+
+
+
+ )
+
+
+
+
+
Parameters
+
+ H_S_T Target pose in cartesian space.
+ solutions Output vector of solutions stored as joint arrays.
+
+
+
+
Returns True if all solutions are reachable, false otherwise.
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1ScrewTheoryIkProblemBuilder-members.html b/classroboticslab_1_1ScrewTheoryIkProblemBuilder-members.html
new file mode 100644
index 000000000..9bf5630f4
--- /dev/null
+++ b/classroboticslab_1_1ScrewTheoryIkProblemBuilder-members.html
@@ -0,0 +1,103 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ScrewTheoryIkProblemBuilder , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1ScrewTheoryIkProblemBuilder.html b/classroboticslab_1_1ScrewTheoryIkProblemBuilder.html
new file mode 100644
index 000000000..6e8f879d0
--- /dev/null
+++ b/classroboticslab_1_1ScrewTheoryIkProblemBuilder.html
@@ -0,0 +1,215 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ScrewTheoryIkProblemBuilder Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Automated IK solution finder.
+ More...
+
+
#include <ScrewTheoryIkProblem.hpp >
+
+
+struct PoeTerm
+ Helper structure that holds the state of a POE term. More...
+
+
+
+
+ScrewTheoryIkProblem::Steps searchSolutions ()
+
+
+void refreshSimplificationState ()
+
+
+void simplify (int depth)
+
+
+void simplifyWithPadenKahanOne (const KDL::Vector &point)
+
+
+void simplifyWithPadenKahanThree (const KDL::Vector &point)
+
+
+void simplifyWithPardosOne ()
+
+
+ScrewTheoryIkSubproblem * trySolve (int depth)
+
+
+
+
+static std::vector< KDL::Vector > searchPoints (const PoeExpression &poe)
+
+
+
+
+PoeExpression poe
+
+
+std::vector< KDL::Vector > points
+
+
+std::vector< KDL::Vector > testPoints
+
+
+std::vector< PoeTerm > poeTerms
+
+
+
+
+static const int MAX_SIMPLIFICATION_DEPTH = 2
+
+
+
+
This class helps to automate the process for configuring a valid IK problem given its geometric data only. It is intended to take care of the generation and configuration of known subproblems, whereas particular joint-space solutions are meant to be computed on runtime by ScrewTheoryIkProblem .
+
+
+
◆ ScrewTheoryIkProblemBuilder()
+
+
+
+
+
+ ScrewTheoryIkProblemBuilder::ScrewTheoryIkProblemBuilder
+ (
+ const PoeExpression &
+ poe )
+
+
+
+
+
Parameters
+
+ poe Product of exponentials (POE) formula.
+
+
+
+
+
+
+
+
+
◆ build()
+
+
+
+
Returns An instance of an IK problem solver if valid, null otherwise.
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1ScrewTheoryIkSubproblem-members.html b/classroboticslab_1_1ScrewTheoryIkSubproblem-members.html
new file mode 100644
index 000000000..321a7988b
--- /dev/null
+++ b/classroboticslab_1_1ScrewTheoryIkSubproblem-members.html
@@ -0,0 +1,95 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::ScrewTheoryIkSubproblem , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1ScrewTheoryIkSubproblem.html b/classroboticslab_1_1ScrewTheoryIkSubproblem.html
new file mode 100644
index 000000000..92e310656
--- /dev/null
+++ b/classroboticslab_1_1ScrewTheoryIkSubproblem.html
@@ -0,0 +1,212 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::ScrewTheoryIkSubproblem Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Interface shared by all IK subproblems found in Screw Theory applied to Robotics.
+ More...
+
+
#include <ScrewTheoryIkProblem.hpp >
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+virtual ~ScrewTheoryIkSubproblem ()=default
+ Destructor.
+
+virtual bool solve (const KDL::Frame &rhs, const KDL::Frame &pointTransform, Solutions &solutions ) const =0
+ Finds a closed geometric solution for this IK subproblem. More...
+
+
+virtual int solutions () const =0
+ Number of local IK solutions.
+
+
+virtual const char * describe () const =0
+ Return a human-readable description of this IK subproblem.
+
+
+
+
Derived classes are considered to be immutable.
+
+
+
◆ solve()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::ScrewTheoryIkSubproblem::solve
+ (
+ const KDL::Frame &
+ rhs ,
+
+
+
+
+ const KDL::Frame &
+ pointTransform ,
+
+
+
+
+ Solutions &
+ solutions
+
+
+
+ )
+ const
+
+
+
+
+pure virtual
+
+
+
+
Given the product of exponentials (POE) formula \( \prod_i e\,^{\hat{\xi}_i\,{\theta_i}} \cdot H_{ST}(0) = H_{ST}(\theta) \) , , invariant and known terms are rearranged to the right side (rhs
) as follows:
+
+\[ \prod_{i=j}^{j+k} e\,^{\hat{\xi}_i\,{\theta_i}} = \left [ \prod_{i=1}^{j-1} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \cdot H_{ST}(\theta) \cdot \left [ H_{ST}(0) \right ]^{-1} \cdot \left [ \prod_{i=j+k+1}^{N} e\,^{\hat{\xi}_i\,{\theta_i}} \right ]^{-1} \]
+
+
where \( j = \{1, 2, ..., N\}, k = \{1, 2, ..., N-1\}, 1 <= j+k <= N \) .
+
Given \( N \) terms in the POE formula, \( j \) of which are unknowns, any characteristic point \( p \) postmultiplying this expression could be rewritten as \( p' \) per:
+
+\[ \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot \prod_{i=j+1}^N e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p = \prod_{i=1}^j e\,^{\hat{\xi}_i\,{\theta_i}} \cdot p' \]
+
+
where pointTransform
is the transformation matrix that produces \( p' \) from \( p \) .
+
Parameters
+
+ rhs Right-hand side of the POE formula prior to being applied to the right-hand side of this subproblem.
+ pointTransform Transformation frame applied to the first (and perhaps only) characteristic point of this subproblem.
+ solutions Output vector of local solutions.
+
+
+
+
Returns True if all solutions are reachable, false otherwise.
+
+
Implemented in roboticslab::PardosGotorFour , roboticslab::PardosGotorThree , roboticslab::PardosGotorTwo , roboticslab::PardosGotorOne , roboticslab::PadenKahanThree , roboticslab::PadenKahanTwo , and roboticslab::PadenKahanOne .
+
+
+
+
The documentation for this class was generated from the following file:
+
+
+
+
+
diff --git a/classroboticslab_1_1ScrewTheoryIkSubproblem.png b/classroboticslab_1_1ScrewTheoryIkSubproblem.png
new file mode 100644
index 000000000..7ccf84c5e
Binary files /dev/null and b/classroboticslab_1_1ScrewTheoryIkSubproblem.png differ
diff --git a/classroboticslab_1_1SpnavSensorDevice-members.html b/classroboticslab_1_1SpnavSensorDevice-members.html
new file mode 100644
index 000000000..b4679299e
--- /dev/null
+++ b/classroboticslab_1_1SpnavSensorDevice-members.html
@@ -0,0 +1,112 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::SpnavSensorDevice , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1SpnavSensorDevice.html b/classroboticslab_1_1SpnavSensorDevice.html
new file mode 100644
index 000000000..40fda1bf3
--- /dev/null
+++ b/classroboticslab_1_1SpnavSensorDevice.html
@@ -0,0 +1,405 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::SpnavSensorDevice Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Represents a spacenav-compatible device, like the SpaceNavigator 6-DOF mouse from 3Dconnexion.
+
+
+
#include <SpnavSensorDevice.hpp >
+
+
+
+
+
+
+
+
+
+
+
+yarp::dev::IAnalogSensor * iAnalogSensor
+
+
+std::vector< double > currentX
+
+
+bool usingPose
+
+
+double gain
+
+
+bool buttonClose
+
+
+bool buttonOpen
+
+
+
+
+
+ICartesianControl * iCartesianControl
+
+
+std::vector< double > data
+
+
+std::vector< bool > fixedAxes
+
+
+int actuatorState
+
+
+
+
+
◆ acquireData()
+
+
+
+
+
+
+
+
+ bool SpnavSensorDevice::acquireData
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ acquireInterfaces()
+
+
+
+
+
+
+
+
+ bool SpnavSensorDevice::acquireInterfaces
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ getActuatorState()
+
+
+
+
+
+
+
+
+ int SpnavSensorDevice::getActuatorState
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ hasValidMovementData()
+
+
+
+
+
+
+
+
+ bool SpnavSensorDevice::hasValidMovementData
+ (
+ )
+ const
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ initialize()
+
+
+
+
+
+
+
+
+ bool SpnavSensorDevice::initialize
+ (
+ bool
+ usingStreamingPreset )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ usingStreamingPreset Whether the cartesian controller supports streaming command presets or not.
+
+
+
+
Returns true on success, false otherwise
+
+
Reimplemented from roboticslab::StreamingDevice .
+
+
+
+
+
◆ sendMovementCommand()
+
+
+
+
+
+
+
+
+ void SpnavSensorDevice::sendMovementCommand
+ (
+ double
+ timestamp )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ transformData()
+
+
+
+
+
+
+
+
+ bool SpnavSensorDevice::transformData
+ (
+ double
+ scaling )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ scaling Scaling factor applied to each data value.
+
+
+
+
Returns true on success, false otherwise
+
+
Reimplemented from roboticslab::StreamingDevice .
+
+
+
+
The documentation for this class was generated from the following files:
+programs/streamingDeviceController/SpnavSensorDevice.hpp
+programs/streamingDeviceController/SpnavSensorDevice.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1SpnavSensorDevice.png b/classroboticslab_1_1SpnavSensorDevice.png
new file mode 100644
index 000000000..67f2a6165
Binary files /dev/null and b/classroboticslab_1_1SpnavSensorDevice.png differ
diff --git a/classroboticslab_1_1StreamResponder-members.html b/classroboticslab_1_1StreamResponder-members.html
new file mode 100644
index 000000000..bc3b845be
--- /dev/null
+++ b/classroboticslab_1_1StreamResponder-members.html
@@ -0,0 +1,95 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::StreamResponder , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1StreamResponder.html b/classroboticslab_1_1StreamResponder.html
new file mode 100644
index 000000000..064caef58
--- /dev/null
+++ b/classroboticslab_1_1StreamResponder.html
@@ -0,0 +1,140 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::StreamResponder Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Responds to streaming command messages.
+
+
+
#include <CartesianControlServer.hpp >
+
+
+
+
+
+
+
+
+using ConsumerFun = void(ICartesianControl::*)(const std::vector< double > &)
+
+
+using BiConsumerFun = void(ICartesianControl::*)(const std::vector< double > &, double)
+
+
+
+
+void handleConsumerCmdMsg (const yarp::os::Bottle &in, ConsumerFun cmd)
+
+
+void handleBiConsumerCmdMsg (const yarp::os::Bottle &in, BiConsumerFun cmd)
+
+
+
The documentation for this class was generated from the following files:
+libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp
+libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1StreamResponder.png b/classroboticslab_1_1StreamResponder.png
new file mode 100644
index 000000000..ff5be2d4e
Binary files /dev/null and b/classroboticslab_1_1StreamResponder.png differ
diff --git a/classroboticslab_1_1StreamingDevice-members.html b/classroboticslab_1_1StreamingDevice-members.html
new file mode 100644
index 000000000..ecbe46115
--- /dev/null
+++ b/classroboticslab_1_1StreamingDevice-members.html
@@ -0,0 +1,105 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::StreamingDevice , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1StreamingDevice.html b/classroboticslab_1_1StreamingDevice.html
new file mode 100644
index 000000000..ed6b4b49d
--- /dev/null
+++ b/classroboticslab_1_1StreamingDevice.html
@@ -0,0 +1,453 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::StreamingDevice Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Abstract class for a YARP streaming device.
+
+
+
#include <StreamingDevice.hpp >
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ICartesianControl * iCartesianControl
+
+
+std::vector< double > data
+
+
+std::vector< bool > fixedAxes
+
+
+int actuatorState
+
+
+
+
+void configureFixedAxes (const yarp::os::Value &v)
+ Stores vector of values representing axes that are always fixed.
+
+
+
+
+friend CentroidTransform
+
+
+
+
+
◆ StreamingDevice()
+
+
+
+
+
+ StreamingDevice::StreamingDevice
+ (
+ yarp::os::Searchable &
+ config )
+
+
+
+
+
Parameters
+
+ config List of options the YARP device should be opened with.
+
+
+
+
+
+
+
+
+
◆ acquireData()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::StreamingDevice::acquireData
+ (
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ acquireInterfaces()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::StreamingDevice::acquireInterfaces
+ (
+ )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ getActuatorState()
+
+
+
+
+
+
+
+
+ virtual int roboticslab::StreamingDevice::getActuatorState
+ (
+ )
+
+
+
+
+
+inline virtual
+
+
+
+
+
+
◆ hasValidMovementData()
+
+
+
+
+
+
+
+
+ bool StreamingDevice::hasValidMovementData
+ (
+ )
+ const
+
+
+
+
+virtual
+
+
+
+
+
+
◆ initialize()
+
+
+
+
+
+
+
+
+ virtual bool roboticslab::StreamingDevice::initialize
+ (
+ bool
+ usingStreamingPreset )
+
+
+
+
+
+inline virtual
+
+
+
+
+
+
◆ sendMovementCommand()
+
+
+
+
+
+
+
+
+ virtual void roboticslab::StreamingDevice::sendMovementCommand
+ (
+ double
+ timestamp )
+
+
+
+
+
+pure virtual
+
+
+
+
+
+
◆ setCartesianControllerHandle()
+
+
+
+
+
+
+
+
+ void roboticslab::StreamingDevice::setCartesianControllerHandle
+ (
+ ICartesianControl *
+ iCartesianControl )
+
+
+
+
+
+inline
+
+
+
+
+
+
◆ transformData()
+
+
+
+
+
+
+
+
+ bool StreamingDevice::transformData
+ (
+ double
+ scaling )
+
+
+
+
+
+virtual
+
+
+
+
+
The documentation for this class was generated from the following files:
+programs/streamingDeviceController/StreamingDevice.hpp
+programs/streamingDeviceController/StreamingDevice.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1StreamingDevice.png b/classroboticslab_1_1StreamingDevice.png
new file mode 100644
index 000000000..ef7383eb7
Binary files /dev/null and b/classroboticslab_1_1StreamingDevice.png differ
diff --git a/classroboticslab_1_1StreamingDeviceController-members.html b/classroboticslab_1_1StreamingDeviceController-members.html
new file mode 100644
index 000000000..6f7850cd0
--- /dev/null
+++ b/classroboticslab_1_1StreamingDeviceController-members.html
@@ -0,0 +1,109 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::StreamingDeviceController , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1StreamingDeviceController.html b/classroboticslab_1_1StreamingDeviceController.html
new file mode 100644
index 000000000..5785806c6
--- /dev/null
+++ b/classroboticslab_1_1StreamingDeviceController.html
@@ -0,0 +1,177 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::StreamingDeviceController Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Sends streaming commands to the cartesian controller from a streaming input device like the 3Dconnexion Space Navigator.
+ More...
+
+
#include <StreamingDeviceController.hpp >
+
+
+
+
+
+
+
+
+bool configure (yarp::os::ResourceFinder &rf) override
+
+
+bool updateModule () override
+
+
+bool interruptModule () override
+
+
+bool close () override
+
+
+double getPeriod () override
+
+
+void onRead (yarp::os::Bottle &bot) override
+
+
+
+
+bool update (double timestamp)
+
+
+
+
+StreamingDevice * streamingDevice
+
+
+yarp::dev::PolyDriver cartesianControlClientDevice
+
+
+roboticslab::ICartesianControl * iCartesianControl
+
+
+yarp::os::BufferedPort< yarp::os::Bottle > proximityPort
+
+
+int thresholdAlertHigh
+
+
+int thresholdAlertLow
+
+
+bool disableSensorsLowLevel
+
+
+yarp::os::BufferedPort< yarp::os::Bottle > centroidPort
+
+
+CentroidTransform centroidTransform
+
+
+yarp::os::BufferedPort< yarp::os::Bottle > syncPort
+
+
+double period
+
+
+double scaling
+
+
+bool isStopped
+
+
+
+
The documentation for this class was generated from the following files:
+
+
+
+
+
diff --git a/classroboticslab_1_1StreamingDeviceController.png b/classroboticslab_1_1StreamingDeviceController.png
new file mode 100644
index 000000000..e2b1b5dcd
Binary files /dev/null and b/classroboticslab_1_1StreamingDeviceController.png differ
diff --git a/classroboticslab_1_1StreamingDeviceFactory-members.html b/classroboticslab_1_1StreamingDeviceFactory-members.html
new file mode 100644
index 000000000..0694febb8
--- /dev/null
+++ b/classroboticslab_1_1StreamingDeviceFactory-members.html
@@ -0,0 +1,90 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::StreamingDeviceFactory , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1StreamingDeviceFactory.html b/classroboticslab_1_1StreamingDeviceFactory.html
new file mode 100644
index 000000000..ac414a9eb
--- /dev/null
+++ b/classroboticslab_1_1StreamingDeviceFactory.html
@@ -0,0 +1,149 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::StreamingDeviceFactory Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Factory class for creating instances of StreamingDevice .
+
+
+
#include <StreamingDevice.hpp >
+
+
+
+
◆ makeDevice()
+
+
+
+
+
+
+
+
+ StreamingDevice * StreamingDeviceFactory::makeDevice
+ (
+ const std::string &
+ deviceName ,
+
+
+
+
+ yarp::os::Searchable &
+ config
+
+
+
+ )
+
+
+
+
+
+static
+
+
+
+
Parameters
+
+ deviceName Name of the device.
+ config List of options the YARP device should be opened with.
+
+
+
+
Returns Pointer to an instance of a StreamingDevice .
+
+
+
+
The documentation for this class was generated from the following files:
+programs/streamingDeviceController/StreamingDevice.hpp
+programs/streamingDeviceController/StreamingDevice.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1WiimoteSensorDevice-members.html b/classroboticslab_1_1WiimoteSensorDevice-members.html
new file mode 100644
index 000000000..144f8851f
--- /dev/null
+++ b/classroboticslab_1_1WiimoteSensorDevice-members.html
@@ -0,0 +1,116 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::WiimoteSensorDevice , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1WiimoteSensorDevice.html b/classroboticslab_1_1WiimoteSensorDevice.html
new file mode 100644
index 000000000..5fa18f216
--- /dev/null
+++ b/classroboticslab_1_1WiimoteSensorDevice.html
@@ -0,0 +1,384 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::WiimoteSensorDevice Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Represents a Wiimote device wrapped as an analog sensor by YARP.
+
+
+
#include <WiimoteSensorDevice.hpp >
+
+
+
+
+
+
+
+
+
+
+ enum cmd_mode { NONE
+, FWD
+, BKWD
+, ROT
+ }
+
+
+
+
+yarp::dev::IAnalogSensor * iAnalogSensor
+
+
+cmd_mode mode
+
+
+std::vector< double > buffer
+
+
+bool usingPose
+
+
+double step
+
+
+
+
+
+ICartesianControl * iCartesianControl
+
+
+std::vector< double > data
+
+
+std::vector< bool > fixedAxes
+
+
+int actuatorState
+
+
+
+
+
◆ acquireData()
+
+
+
+
+
+
+
+
+ bool WiimoteSensorDevice::acquireData
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ acquireInterfaces()
+
+
+
+
+
+
+
+
+ bool WiimoteSensorDevice::acquireInterfaces
+ (
+ )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ hasValidMovementData()
+
+
+
+
+
+
+
+
+ bool WiimoteSensorDevice::hasValidMovementData
+ (
+ )
+ const
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ initialize()
+
+
+
+
+
+
+
+
+ bool WiimoteSensorDevice::initialize
+ (
+ bool
+ usingStreamingPreset )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ usingStreamingPreset Whether the cartesian controller supports streaming command presets or not.
+
+
+
+
Returns true on success, false otherwise
+
+
Reimplemented from roboticslab::StreamingDevice .
+
+
+
+
+
◆ sendMovementCommand()
+
+
+
+
+
+
+
+
+ void WiimoteSensorDevice::sendMovementCommand
+ (
+ double
+ timestamp )
+
+
+
+
+
+override virtual
+
+
+
+
+
+
◆ transformData()
+
+
+
+
+
+
+
+
+ bool WiimoteSensorDevice::transformData
+ (
+ double
+ scaling )
+
+
+
+
+
+override virtual
+
+
+
+
Parameters
+
+ scaling Scaling factor applied to each data value.
+
+
+
+
Returns true on success, false otherwise
+
+
Reimplemented from roboticslab::StreamingDevice .
+
+
+
+
The documentation for this class was generated from the following files:
+programs/streamingDeviceController/WiimoteSensorDevice.hpp
+programs/streamingDeviceController/WiimoteSensorDevice.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1WiimoteSensorDevice.png b/classroboticslab_1_1WiimoteSensorDevice.png
new file mode 100644
index 000000000..3b941ad73
Binary files /dev/null and b/classroboticslab_1_1WiimoteSensorDevice.png differ
diff --git a/classroboticslab_1_1test_1_1AsibotSolverTestFromFile-members.html b/classroboticslab_1_1test_1_1AsibotSolverTestFromFile-members.html
new file mode 100644
index 000000000..0c49a85ca
--- /dev/null
+++ b/classroboticslab_1_1test_1_1AsibotSolverTestFromFile-members.html
@@ -0,0 +1,94 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::test::AsibotSolverTestFromFile , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1test_1_1AsibotSolverTestFromFile.html b/classroboticslab_1_1test_1_1AsibotSolverTestFromFile.html
new file mode 100644
index 000000000..c93566331
--- /dev/null
+++ b/classroboticslab_1_1test_1_1AsibotSolverTestFromFile.html
@@ -0,0 +1,130 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::test::AsibotSolverTestFromFile Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Tests AsibotSolver ikin from loaded configuration file.
+
+
+
+
+
+
+
+
+
+void SetUp () override
+
+
+void TearDown () override
+
+
+
+
+static const double EPS_CART = 1e-6
+
+
+static const double EPS_JOINT = 1e-3
+
+
+
The documentation for this class was generated from the following file:
+tests/testAsibotSolverFromFile.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1test_1_1AsibotSolverTestFromFile.png b/classroboticslab_1_1test_1_1AsibotSolverTestFromFile.png
new file mode 100644
index 000000000..7644a68ba
Binary files /dev/null and b/classroboticslab_1_1test_1_1AsibotSolverTestFromFile.png differ
diff --git a/classroboticslab_1_1test_1_1BasicCartesianControlTest-members.html b/classroboticslab_1_1test_1_1BasicCartesianControlTest-members.html
new file mode 100644
index 000000000..2f6619dda
--- /dev/null
+++ b/classroboticslab_1_1test_1_1BasicCartesianControlTest-members.html
@@ -0,0 +1,92 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::test::BasicCartesianControlTest , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1test_1_1BasicCartesianControlTest.html b/classroboticslab_1_1test_1_1BasicCartesianControlTest.html
new file mode 100644
index 000000000..49e4e922a
--- /dev/null
+++ b/classroboticslab_1_1test_1_1BasicCartesianControlTest.html
@@ -0,0 +1,120 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::test::BasicCartesianControlTest Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Tests BasicCartesianControl ikin and idyn on a simple mechanism.
+
+
+
+
+
+
+
+
+
+void SetUp () override
+
+
+void TearDown () override
+
+
+
The documentation for this class was generated from the following file:
+tests/testBasicCartesianControl.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1test_1_1BasicCartesianControlTest.png b/classroboticslab_1_1test_1_1BasicCartesianControlTest.png
new file mode 100644
index 000000000..9b54bc79c
Binary files /dev/null and b/classroboticslab_1_1test_1_1BasicCartesianControlTest.png differ
diff --git a/classroboticslab_1_1test_1_1KdlSolverTest-members.html b/classroboticslab_1_1test_1_1KdlSolverTest-members.html
new file mode 100644
index 000000000..80583bbaf
--- /dev/null
+++ b/classroboticslab_1_1test_1_1KdlSolverTest-members.html
@@ -0,0 +1,93 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::test::KdlSolverTest , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1test_1_1KdlSolverTest.html b/classroboticslab_1_1test_1_1KdlSolverTest.html
new file mode 100644
index 000000000..67e26ea73
--- /dev/null
+++ b/classroboticslab_1_1test_1_1KdlSolverTest.html
@@ -0,0 +1,127 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::test::KdlSolverTest Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Tests KdlSolver ikin and idyn on a simple mechanism.
+
+
+
+
+
+
+
+
+
+void SetUp () override
+
+
+void TearDown () override
+
+
+
+
+static constexpr double eps = 1e-9
+
+
+
The documentation for this class was generated from the following file:
+tests/testKdlSolver.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1test_1_1KdlSolverTest.png b/classroboticslab_1_1test_1_1KdlSolverTest.png
new file mode 100644
index 000000000..fd3e6dedb
Binary files /dev/null and b/classroboticslab_1_1test_1_1KdlSolverTest.png differ
diff --git a/classroboticslab_1_1test_1_1KdlSolverTestFromFile-members.html b/classroboticslab_1_1test_1_1KdlSolverTestFromFile-members.html
new file mode 100644
index 000000000..15c32c028
--- /dev/null
+++ b/classroboticslab_1_1test_1_1KdlSolverTestFromFile-members.html
@@ -0,0 +1,92 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::test::KdlSolverTestFromFile , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1test_1_1KdlSolverTestFromFile.html b/classroboticslab_1_1test_1_1KdlSolverTestFromFile.html
new file mode 100644
index 000000000..5d9aca36c
--- /dev/null
+++ b/classroboticslab_1_1test_1_1KdlSolverTestFromFile.html
@@ -0,0 +1,120 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::test::KdlSolverTestFromFile Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Tests KdlSolver ikin and idyn on a simple mechanism.
+
+
+
+
+
+
+
+
+
+void SetUp () override
+
+
+void TearDown () override
+
+
+
The documentation for this class was generated from the following file:
+tests/testKdlSolverFromFile.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1test_1_1KdlSolverTestFromFile.png b/classroboticslab_1_1test_1_1KdlSolverTestFromFile.png
new file mode 100644
index 000000000..1d17e6346
Binary files /dev/null and b/classroboticslab_1_1test_1_1KdlSolverTestFromFile.png differ
diff --git a/classroboticslab_1_1test_1_1KinRepresentationTest-members.html b/classroboticslab_1_1test_1_1KinRepresentationTest-members.html
new file mode 100644
index 000000000..051f3ce8a
--- /dev/null
+++ b/classroboticslab_1_1test_1_1KinRepresentationTest-members.html
@@ -0,0 +1,91 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::test::KinRepresentationTest , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1test_1_1KinRepresentationTest.html b/classroboticslab_1_1test_1_1KinRepresentationTest.html
new file mode 100644
index 000000000..120612b79
--- /dev/null
+++ b/classroboticslab_1_1test_1_1KinRepresentationTest.html
@@ -0,0 +1,117 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::test::KinRepresentationTest Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Tests KinRepresentation .
+
+
+
+
+
+
+
+
+
+void SetUp () override
+
+
+void TearDown () override
+
+
+
+
+static const double EPS = 1e-9
+
+
+
The documentation for this class was generated from the following file:
+tests/testKinRepresentation.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1test_1_1KinRepresentationTest.png b/classroboticslab_1_1test_1_1KinRepresentationTest.png
new file mode 100644
index 000000000..e25ca5fda
Binary files /dev/null and b/classroboticslab_1_1test_1_1KinRepresentationTest.png differ
diff --git a/classroboticslab_1_1test_1_1ScrewTheoryTest-members.html b/classroboticslab_1_1test_1_1ScrewTheoryTest-members.html
new file mode 100644
index 000000000..32cd33577
--- /dev/null
+++ b/classroboticslab_1_1test_1_1ScrewTheoryTest-members.html
@@ -0,0 +1,109 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Member List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is the complete list of members for roboticslab::test::ScrewTheoryTest , including all inherited members.
+
+
+
+
+
diff --git a/classroboticslab_1_1test_1_1ScrewTheoryTest.html b/classroboticslab_1_1test_1_1ScrewTheoryTest.html
new file mode 100644
index 000000000..f9a1b31cf
--- /dev/null
+++ b/classroboticslab_1_1test_1_1ScrewTheoryTest.html
@@ -0,0 +1,181 @@
+
+
+
+
+
+
+
+kinematics-dynamics: roboticslab::test::ScrewTheoryTest Class Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Tests classes related to Screw Theory.
+
+
+
+
+
+
+
+
+
+void SetUp () override
+
+
+void TearDown () override
+
+
+
+
+static KDL::JntArray fillJointValues (int size, double value)
+
+
+static KDL::Chain makeTeoRightArmKinematicsFromDH ()
+
+
+static PoeExpression makeTeoRightArmKinematicsFromPoE ()
+
+
+static KDL::Chain makeTeoRightLegKinematicsFromDH ()
+
+
+static PoeExpression makeTeoRightLegKinematicsFromPoE ()
+
+
+static KDL::Chain makeAbbIrb120KinematicsFromDH ()
+
+
+static PoeExpression makeAbbIrb120KinematicsFromPoE ()
+
+
+static KDL::Chain makePumaKinematicsFromDH ()
+
+
+static PoeExpression makePumaKinematicsFromPoE ()
+
+
+static KDL::Chain makeStanfordKinematicsFromDH ()
+
+
+static PoeExpression makeStanfordKinematicsFromPoE ()
+
+
+static KDL::Chain makeAbbIrb910scKinematicsFromDH ()
+
+
+static PoeExpression makeAbbIrb910scKinematicsFromPoE ()
+
+
+static KDL::Chain makeAbbIrb6620lxFromDh ()
+
+
+static PoeExpression makeAbbIrb6620lxFromPoE ()
+
+
+static void checkSolutions (const ScrewTheoryIkSubproblem::Solutions &actual, const ScrewTheoryIkSubproblem::Solutions &expected)
+
+
+static void checkRobotKinematics (const KDL::Chain &chain, const PoeExpression &poe, int soln)
+
+
+static int findTargetConfiguration (const ScrewTheoryIkProblem::Solutions &solutions, const KDL::JntArray &target)
+
+
+
The documentation for this class was generated from the following file:
+tests/testScrewTheory.cpp
+
+
+
+
+
+
diff --git a/classroboticslab_1_1test_1_1ScrewTheoryTest.png b/classroboticslab_1_1test_1_1ScrewTheoryTest.png
new file mode 100644
index 000000000..a45cb9576
Binary files /dev/null and b/classroboticslab_1_1test_1_1ScrewTheoryTest.png differ
diff --git a/closed.png b/closed.png
new file mode 100644
index 000000000..98cc2c909
Binary files /dev/null and b/closed.png differ
diff --git a/dir_1309cee196689adea45d11f37b8ed78d.html b/dir_1309cee196689adea45d11f37b8ed78d.html
new file mode 100644
index 000000000..cd018b408
--- /dev/null
+++ b/dir_1309cee196689adea45d11f37b8ed78d.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: examples/cpp/exampleYarpTinyMath Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_15768b185b6e8b816adf0c89b30bfe65.html b/dir_15768b185b6e8b816adf0c89b30bfe65.html
new file mode 100644
index 000000000..a83e193ef
--- /dev/null
+++ b/dir_15768b185b6e8b816adf0c89b30bfe65.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/streamingDeviceController Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_17d4fdefd982442a79427fe9d98568ae.html b/dir_17d4fdefd982442a79427fe9d98568ae.html
new file mode 100644
index 000000000..94c1449ec
--- /dev/null
+++ b/dir_17d4fdefd982442a79427fe9d98568ae.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/AsibotSolver Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_1d6bf2fb1d016ec6b197455d198424da.html b/dir_1d6bf2fb1d016ec6b197455d198424da.html
new file mode 100644
index 000000000..76bdcf320
--- /dev/null
+++ b/dir_1d6bf2fb1d016ec6b197455d198424da.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/CartesianControlServer Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_25c7435138c3ee6ac4c9babe8db7ecf9.html b/dir_25c7435138c3ee6ac4c9babe8db7ecf9.html
new file mode 100644
index 000000000..f13921719
--- /dev/null
+++ b/dir_25c7435138c3ee6ac4c9babe8db7ecf9.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/ftCompensation Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_387a94d47062d4e808cee3de63065fcd.html b/dir_387a94d47062d4e808cee3de63065fcd.html
new file mode 100644
index 000000000..95c0b4a06
--- /dev/null
+++ b/dir_387a94d47062d4e808cee3de63065fcd.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/haarDetectionController Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_487dfb5d6e3e441bbdba0aef32150596.html b/dir_487dfb5d6e3e441bbdba0aef32150596.html
new file mode 100644
index 000000000..5595bbe32
--- /dev/null
+++ b/dir_487dfb5d6e3e441bbdba0aef32150596.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/KdlTreeSolver Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_49e6bd22918cee17ddc14df5c827b459.html b/dir_49e6bd22918cee17ddc14df5c827b459.html
new file mode 100644
index 000000000..c3d09249a
--- /dev/null
+++ b/dir_49e6bd22918cee17ddc14df5c827b459.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/KinematicRepresentationLib Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_4e03118d576dd8e71b5dbe7081ce822d.html b/dir_4e03118d576dd8e71b5dbe7081ce822d.html
new file mode 100644
index 000000000..947897201
--- /dev/null
+++ b/dir_4e03118d576dd8e71b5dbe7081ce822d.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs/keyboardController Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_4f22d2c14b4bbce8870d40d37a8ebee2.html b/dir_4f22d2c14b4bbce8870d40d37a8ebee2.html
new file mode 100644
index 000000000..20635eb03
--- /dev/null
+++ b/dir_4f22d2c14b4bbce8870d40d37a8ebee2.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/CartesianControlClient Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_51393f38608496e2be890df69ecb13f1.html b/dir_51393f38608496e2be890df69ecb13f1.html
new file mode 100644
index 000000000..13d8e2bcc
--- /dev/null
+++ b/dir_51393f38608496e2be890df69ecb13f1.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: examples/cpp/exampleCartesianControlClient Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_59425e443f801f1f2fd8bbe4959a3ccf.html b/dir_59425e443f801f1f2fd8bbe4959a3ccf.html
new file mode 100644
index 000000000..e91bcc889
--- /dev/null
+++ b/dir_59425e443f801f1f2fd8bbe4959a3ccf.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: tests Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_75b15c0a6e736aa4482428563186dcc3.html b/dir_75b15c0a6e736aa4482428563186dcc3.html
new file mode 100644
index 000000000..59cf97cb2
--- /dev/null
+++ b/dir_75b15c0a6e736aa4482428563186dcc3.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/BasicCartesianControl Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_7b0a5d1507c7f681cbfa1deb5990c6ea.html b/dir_7b0a5d1507c7f681cbfa1deb5990c6ea.html
new file mode 100644
index 000000000..85b6a73ac
--- /dev/null
+++ b/dir_7b0a5d1507c7f681cbfa1deb5990c6ea.html
@@ -0,0 +1,89 @@
+
+
+
+
+
+
+
+kinematics-dynamics: programs Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_8976e5d03119516083eaca3ddca61311.html b/dir_8976e5d03119516083eaca3ddca61311.html
new file mode 100644
index 000000000..bda14f3c4
--- /dev/null
+++ b/dir_8976e5d03119516083eaca3ddca61311.html
@@ -0,0 +1,89 @@
+
+
+
+
+
+
+
+kinematics-dynamics: examples/cpp Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_a52c22a8b79e169194d9c4bbc7c46367.html b/dir_a52c22a8b79e169194d9c4bbc7c46367.html
new file mode 100644
index 000000000..5056f5462
--- /dev/null
+++ b/dir_a52c22a8b79e169194d9c4bbc7c46367.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: doc/build Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_af0bcd1d9c8bf0be37b473fa7c916aed.html b/dir_af0bcd1d9c8bf0be37b473fa7c916aed.html
new file mode 100644
index 000000000..c45a4791a
--- /dev/null
+++ b/dir_af0bcd1d9c8bf0be37b473fa7c916aed.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: examples/cpp/exampleScrewTheoryTrajectory Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_bbdaf73c7839257653cfce0d61f80663.html b/dir_bbdaf73c7839257653cfce0d61f80663.html
new file mode 100644
index 000000000..8a20f1b49
--- /dev/null
+++ b/dir_bbdaf73c7839257653cfce0d61f80663.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpTinyMathLib Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_bc0718b08fb2015b8e59c47b2805f60c.html b/dir_bc0718b08fb2015b8e59c47b2805f60c.html
new file mode 100644
index 000000000..c31367233
--- /dev/null
+++ b/dir_bc0718b08fb2015b8e59c47b2805f60c.html
@@ -0,0 +1,89 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_bfeb62c1259859bb2c79e7bf678570c2.html b/dir_bfeb62c1259859bb2c79e7bf678570c2.html
new file mode 100644
index 000000000..f55a5d0cd
--- /dev/null
+++ b/dir_bfeb62c1259859bb2c79e7bf678570c2.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins/KdlSolver Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_d050070cc3e4bbd91d897ff8856046e0.html b/dir_d050070cc3e4bbd91d897ff8856046e0.html
new file mode 100644
index 000000000..7d2bd3ad4
--- /dev/null
+++ b/dir_d050070cc3e4bbd91d897ff8856046e0.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: examples/python Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_d28a4824dc47e487b107a5db32ef43c4.html b/dir_d28a4824dc47e487b107a5db32ef43c4.html
new file mode 100644
index 000000000..7bf34fe4b
--- /dev/null
+++ b/dir_d28a4824dc47e487b107a5db32ef43c4.html
@@ -0,0 +1,91 @@
+
+
+
+
+
+
+
+kinematics-dynamics: examples Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_dfab584b716eccfc00a175735f16fb62.html b/dir_dfab584b716eccfc00a175735f16fb62.html
new file mode 100644
index 000000000..64e8b923f
--- /dev/null
+++ b/dir_dfab584b716eccfc00a175735f16fb62.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/ScrewTheoryLib Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_e68e8157741866f444e17edd764ebbae.html b/dir_e68e8157741866f444e17edd764ebbae.html
new file mode 100644
index 000000000..607972701
--- /dev/null
+++ b/dir_e68e8157741866f444e17edd764ebbae.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: doc Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_f528a5afad561ac522bac3a41785be2e.html b/dir_f528a5afad561ac522bac3a41785be2e.html
new file mode 100644
index 000000000..98de5bb84
--- /dev/null
+++ b/dir_f528a5afad561ac522bac3a41785be2e.html
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/KdlVectorConverterLib Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dir_f98f57c538677273d6e3d52c55355c28.html b/dir_f98f57c538677273d6e3d52c55355c28.html
new file mode 100644
index 000000000..68cae4687
--- /dev/null
+++ b/dir_f98f57c538677273d6e3d52c55355c28.html
@@ -0,0 +1,98 @@
+
+
+
+
+
+
+
+kinematics-dynamics: libraries/YarpPlugins Directory Reference
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/doc.png b/doc.png
new file mode 100644
index 000000000..17edabff9
Binary files /dev/null and b/doc.png differ
diff --git a/doxygen.css b/doxygen.css
new file mode 100644
index 000000000..ffbff0224
--- /dev/null
+++ b/doxygen.css
@@ -0,0 +1,1793 @@
+/* The standard CSS for doxygen 1.9.1 */
+
+body, table, div, p, dl {
+ font: 400 14px/22px Roboto,sans-serif;
+}
+
+p.reference, p.definition {
+ font: 400 14px/22px Roboto,sans-serif;
+}
+
+/* @group Heading Levels */
+
+h1.groupheader {
+ font-size: 150%;
+}
+
+.title {
+ font: 400 14px/28px Roboto,sans-serif;
+ font-size: 150%;
+ font-weight: bold;
+ margin: 10px 2px;
+}
+
+h2.groupheader {
+ border-bottom: 1px solid #879ECB;
+ color: #354C7B;
+ font-size: 150%;
+ font-weight: normal;
+ margin-top: 1.75em;
+ padding-top: 8px;
+ padding-bottom: 4px;
+ width: 100%;
+}
+
+h3.groupheader {
+ font-size: 100%;
+}
+
+h1, h2, h3, h4, h5, h6 {
+ -webkit-transition: text-shadow 0.5s linear;
+ -moz-transition: text-shadow 0.5s linear;
+ -ms-transition: text-shadow 0.5s linear;
+ -o-transition: text-shadow 0.5s linear;
+ transition: text-shadow 0.5s linear;
+ margin-right: 15px;
+}
+
+h1.glow, h2.glow, h3.glow, h4.glow, h5.glow, h6.glow {
+ text-shadow: 0 0 15px cyan;
+}
+
+dt {
+ font-weight: bold;
+}
+
+ul.multicol {
+ -moz-column-gap: 1em;
+ -webkit-column-gap: 1em;
+ column-gap: 1em;
+ -moz-column-count: 3;
+ -webkit-column-count: 3;
+ column-count: 3;
+}
+
+p.startli, p.startdd {
+ margin-top: 2px;
+}
+
+th p.starttd, th p.intertd, th p.endtd {
+ font-size: 100%;
+ font-weight: 700;
+}
+
+p.starttd {
+ margin-top: 0px;
+}
+
+p.endli {
+ margin-bottom: 0px;
+}
+
+p.enddd {
+ margin-bottom: 4px;
+}
+
+p.endtd {
+ margin-bottom: 2px;
+}
+
+p.interli {
+}
+
+p.interdd {
+}
+
+p.intertd {
+}
+
+/* @end */
+
+caption {
+ font-weight: bold;
+}
+
+span.legend {
+ font-size: 70%;
+ text-align: center;
+}
+
+h3.version {
+ font-size: 90%;
+ text-align: center;
+}
+
+div.navtab {
+ border-right: 1px solid #A3B4D7;
+ padding-right: 15px;
+ text-align: right;
+ line-height: 110%;
+}
+
+div.navtab table {
+ border-spacing: 0;
+}
+
+td.navtab {
+ padding-right: 6px;
+ padding-left: 6px;
+}
+td.navtabHL {
+ background-image: url('tab_a.png');
+ background-repeat:repeat-x;
+ padding-right: 6px;
+ padding-left: 6px;
+}
+
+td.navtabHL a, td.navtabHL a:visited {
+ color: #fff;
+ text-shadow: 0px 1px 1px rgba(0, 0, 0, 1.0);
+}
+
+a.navtab {
+ font-weight: bold;
+}
+
+div.qindex{
+ text-align: center;
+ width: 100%;
+ line-height: 140%;
+ font-size: 130%;
+ color: #A0A0A0;
+}
+
+dt.alphachar{
+ font-size: 180%;
+ font-weight: bold;
+}
+
+.alphachar a{
+ color: black;
+}
+
+.alphachar a:hover, .alphachar a:visited{
+ text-decoration: none;
+}
+
+.classindex dl {
+ padding: 25px;
+ column-count:1
+}
+
+.classindex dd {
+ display:inline-block;
+ margin-left: 50px;
+ width: 90%;
+ line-height: 1.15em;
+}
+
+.classindex dl.odd {
+ background-color: #F8F9FC;
+}
+
+@media(min-width: 1120px) {
+ .classindex dl {
+ column-count:2
+ }
+}
+
+@media(min-width: 1320px) {
+ .classindex dl {
+ column-count:3
+ }
+}
+
+
+/* @group Link Styling */
+
+a {
+ color: #3D578C;
+ font-weight: normal;
+ text-decoration: none;
+}
+
+.contents a:visited {
+ color: #4665A2;
+}
+
+a:hover {
+ text-decoration: underline;
+}
+
+.contents a.qindexHL:visited {
+ color: #FFFFFF;
+}
+
+a.el {
+ font-weight: bold;
+}
+
+a.elRef {
+}
+
+a.code, a.code:visited, a.line, a.line:visited {
+ color: #4665A2;
+}
+
+a.codeRef, a.codeRef:visited, a.lineRef, a.lineRef:visited {
+ color: #4665A2;
+}
+
+/* @end */
+
+dl.el {
+ margin-left: -1cm;
+}
+
+ul {
+ overflow: hidden; /*Fixed: list item bullets overlap floating elements*/
+}
+
+#side-nav ul {
+ overflow: visible; /* reset ul rule for scroll bar in GENERATE_TREEVIEW window */
+}
+
+#main-nav ul {
+ overflow: visible; /* reset ul rule for the navigation bar drop down lists */
+}
+
+.fragment {
+ text-align: left;
+ direction: ltr;
+ overflow-x: auto; /*Fixed: fragment lines overlap floating elements*/
+ overflow-y: hidden;
+}
+
+pre.fragment {
+ border: 1px solid #C4CFE5;
+ background-color: #FBFCFD;
+ padding: 4px 6px;
+ margin: 4px 8px 4px 2px;
+ overflow: auto;
+ word-wrap: break-word;
+ font-size: 9pt;
+ line-height: 125%;
+ font-family: monospace, fixed;
+ font-size: 105%;
+}
+
+div.fragment {
+ padding: 0 0 1px 0; /*Fixed: last line underline overlap border*/
+ margin: 4px 8px 4px 2px;
+ background-color: #FBFCFD;
+ border: 1px solid #C4CFE5;
+}
+
+div.line {
+ font-family: monospace, fixed;
+ font-size: 13px;
+ min-height: 13px;
+ line-height: 1.0;
+ text-wrap: unrestricted;
+ white-space: -moz-pre-wrap; /* Moz */
+ white-space: -pre-wrap; /* Opera 4-6 */
+ white-space: -o-pre-wrap; /* Opera 7 */
+ white-space: pre-wrap; /* CSS3 */
+ word-wrap: break-word; /* IE 5.5+ */
+ text-indent: -53px;
+ padding-left: 53px;
+ padding-bottom: 0px;
+ margin: 0px;
+ -webkit-transition-property: background-color, box-shadow;
+ -webkit-transition-duration: 0.5s;
+ -moz-transition-property: background-color, box-shadow;
+ -moz-transition-duration: 0.5s;
+ -ms-transition-property: background-color, box-shadow;
+ -ms-transition-duration: 0.5s;
+ -o-transition-property: background-color, box-shadow;
+ -o-transition-duration: 0.5s;
+ transition-property: background-color, box-shadow;
+ transition-duration: 0.5s;
+}
+
+div.line:after {
+ content:"\000A";
+ white-space: pre;
+}
+
+div.line.glow {
+ background-color: cyan;
+ box-shadow: 0 0 10px cyan;
+}
+
+
+span.lineno {
+ padding-right: 4px;
+ text-align: right;
+ border-right: 2px solid #0F0;
+ background-color: #E8E8E8;
+ white-space: pre;
+}
+span.lineno a {
+ background-color: #D8D8D8;
+}
+
+span.lineno a:hover {
+ background-color: #C8C8C8;
+}
+
+.lineno {
+ -webkit-touch-callout: none;
+ -webkit-user-select: none;
+ -khtml-user-select: none;
+ -moz-user-select: none;
+ -ms-user-select: none;
+ user-select: none;
+}
+
+div.ah, span.ah {
+ background-color: black;
+ font-weight: bold;
+ color: #FFFFFF;
+ margin-bottom: 3px;
+ margin-top: 3px;
+ padding: 0.2em;
+ border: solid thin #333;
+ border-radius: 0.5em;
+ -webkit-border-radius: .5em;
+ -moz-border-radius: .5em;
+ box-shadow: 2px 2px 3px #999;
+ -webkit-box-shadow: 2px 2px 3px #999;
+ -moz-box-shadow: rgba(0, 0, 0, 0.15) 2px 2px 2px;
+ background-image: -webkit-gradient(linear, left top, left bottom, from(#eee), to(#000),color-stop(0.3, #444));
+ background-image: -moz-linear-gradient(center top, #eee 0%, #444 40%, #000 110%);
+}
+
+div.classindex ul {
+ list-style: none;
+ padding-left: 0;
+}
+
+div.classindex span.ai {
+ display: inline-block;
+}
+
+div.groupHeader {
+ margin-left: 16px;
+ margin-top: 12px;
+ font-weight: bold;
+}
+
+div.groupText {
+ margin-left: 16px;
+ font-style: italic;
+}
+
+body {
+ background-color: white;
+ color: black;
+ margin: 0;
+}
+
+div.contents {
+ margin-top: 10px;
+ margin-left: 12px;
+ margin-right: 8px;
+}
+
+td.indexkey {
+ background-color: #EBEFF6;
+ font-weight: bold;
+ border: 1px solid #C4CFE5;
+ margin: 2px 0px 2px 0;
+ padding: 2px 10px;
+ white-space: nowrap;
+ vertical-align: top;
+}
+
+td.indexvalue {
+ background-color: #EBEFF6;
+ border: 1px solid #C4CFE5;
+ padding: 2px 10px;
+ margin: 2px 0px;
+}
+
+tr.memlist {
+ background-color: #EEF1F7;
+}
+
+p.formulaDsp {
+ text-align: center;
+}
+
+img.formulaDsp {
+
+}
+
+img.formulaInl, img.inline {
+ vertical-align: middle;
+}
+
+div.center {
+ text-align: center;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ padding: 0px;
+}
+
+div.center img {
+ border: 0px;
+}
+
+address.footer {
+ text-align: right;
+ padding-right: 12px;
+}
+
+img.footer {
+ border: 0px;
+ vertical-align: middle;
+}
+
+/* @group Code Colorization */
+
+span.keyword {
+ color: #008000
+}
+
+span.keywordtype {
+ color: #604020
+}
+
+span.keywordflow {
+ color: #e08000
+}
+
+span.comment {
+ color: #800000
+}
+
+span.preprocessor {
+ color: #806020
+}
+
+span.stringliteral {
+ color: #002080
+}
+
+span.charliteral {
+ color: #008080
+}
+
+span.vhdldigit {
+ color: #ff00ff
+}
+
+span.vhdlchar {
+ color: #000000
+}
+
+span.vhdlkeyword {
+ color: #700070
+}
+
+span.vhdllogic {
+ color: #ff0000
+}
+
+blockquote {
+ background-color: #F7F8FB;
+ border-left: 2px solid #9CAFD4;
+ margin: 0 24px 0 4px;
+ padding: 0 12px 0 16px;
+}
+
+blockquote.DocNodeRTL {
+ border-left: 0;
+ border-right: 2px solid #9CAFD4;
+ margin: 0 4px 0 24px;
+ padding: 0 16px 0 12px;
+}
+
+/* @end */
+
+/*
+.search {
+ color: #003399;
+ font-weight: bold;
+}
+
+form.search {
+ margin-bottom: 0px;
+ margin-top: 0px;
+}
+
+input.search {
+ font-size: 75%;
+ color: #000080;
+ font-weight: normal;
+ background-color: #e8eef2;
+}
+*/
+
+td.tiny {
+ font-size: 75%;
+}
+
+.dirtab {
+ padding: 4px;
+ border-collapse: collapse;
+ border: 1px solid #A3B4D7;
+}
+
+th.dirtab {
+ background: #EBEFF6;
+ font-weight: bold;
+}
+
+hr {
+ height: 0px;
+ border: none;
+ border-top: 1px solid #4A6AAA;
+}
+
+hr.footer {
+ height: 1px;
+}
+
+/* @group Member Descriptions */
+
+table.memberdecls {
+ border-spacing: 0px;
+ padding: 0px;
+}
+
+.memberdecls td, .fieldtable tr {
+ -webkit-transition-property: background-color, box-shadow;
+ -webkit-transition-duration: 0.5s;
+ -moz-transition-property: background-color, box-shadow;
+ -moz-transition-duration: 0.5s;
+ -ms-transition-property: background-color, box-shadow;
+ -ms-transition-duration: 0.5s;
+ -o-transition-property: background-color, box-shadow;
+ -o-transition-duration: 0.5s;
+ transition-property: background-color, box-shadow;
+ transition-duration: 0.5s;
+}
+
+.memberdecls td.glow, .fieldtable tr.glow {
+ background-color: cyan;
+ box-shadow: 0 0 15px cyan;
+}
+
+.mdescLeft, .mdescRight,
+.memItemLeft, .memItemRight,
+.memTemplItemLeft, .memTemplItemRight, .memTemplParams {
+ background-color: #F9FAFC;
+ border: none;
+ margin: 4px;
+ padding: 1px 0 0 8px;
+}
+
+.mdescLeft, .mdescRight {
+ padding: 0px 8px 4px 8px;
+ color: #555;
+}
+
+.memSeparator {
+ border-bottom: 1px solid #DEE4F0;
+ line-height: 1px;
+ margin: 0px;
+ padding: 0px;
+}
+
+.memItemLeft, .memTemplItemLeft {
+ white-space: nowrap;
+}
+
+.memItemRight, .memTemplItemRight {
+ width: 100%;
+}
+
+.memTemplParams {
+ color: #4665A2;
+ white-space: nowrap;
+ font-size: 80%;
+}
+
+/* @end */
+
+/* @group Member Details */
+
+/* Styles for detailed member documentation */
+
+.memtitle {
+ padding: 8px;
+ border-top: 1px solid #A8B8D9;
+ border-left: 1px solid #A8B8D9;
+ border-right: 1px solid #A8B8D9;
+ border-top-right-radius: 4px;
+ border-top-left-radius: 4px;
+ margin-bottom: -1px;
+ background-image: url('nav_f.png');
+ background-repeat: repeat-x;
+ background-color: #E2E8F2;
+ line-height: 1.25;
+ font-weight: 300;
+ float:left;
+}
+
+.permalink
+{
+ font-size: 65%;
+ display: inline-block;
+ vertical-align: middle;
+}
+
+.memtemplate {
+ font-size: 80%;
+ color: #4665A2;
+ font-weight: normal;
+ margin-left: 9px;
+}
+
+.memnav {
+ background-color: #EBEFF6;
+ border: 1px solid #A3B4D7;
+ text-align: center;
+ margin: 2px;
+ margin-right: 15px;
+ padding: 2px;
+}
+
+.mempage {
+ width: 100%;
+}
+
+.memitem {
+ padding: 0;
+ margin-bottom: 10px;
+ margin-right: 5px;
+ -webkit-transition: box-shadow 0.5s linear;
+ -moz-transition: box-shadow 0.5s linear;
+ -ms-transition: box-shadow 0.5s linear;
+ -o-transition: box-shadow 0.5s linear;
+ transition: box-shadow 0.5s linear;
+ display: table !important;
+ width: 100%;
+}
+
+.memitem.glow {
+ box-shadow: 0 0 15px cyan;
+}
+
+.memname {
+ font-weight: 400;
+ margin-left: 6px;
+}
+
+.memname td {
+ vertical-align: bottom;
+}
+
+.memproto, dl.reflist dt {
+ border-top: 1px solid #A8B8D9;
+ border-left: 1px solid #A8B8D9;
+ border-right: 1px solid #A8B8D9;
+ padding: 6px 0px 6px 0px;
+ color: #253555;
+ font-weight: bold;
+ text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9);
+ background-color: #DFE5F1;
+ /* opera specific markup */
+ box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15);
+ border-top-right-radius: 4px;
+ /* firefox specific markup */
+ -moz-box-shadow: rgba(0, 0, 0, 0.15) 5px 5px 5px;
+ -moz-border-radius-topright: 4px;
+ /* webkit specific markup */
+ -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15);
+ -webkit-border-top-right-radius: 4px;
+
+}
+
+.overload {
+ font-family: "courier new",courier,monospace;
+ font-size: 65%;
+}
+
+.memdoc, dl.reflist dd {
+ border-bottom: 1px solid #A8B8D9;
+ border-left: 1px solid #A8B8D9;
+ border-right: 1px solid #A8B8D9;
+ padding: 6px 10px 2px 10px;
+ background-color: #FBFCFD;
+ border-top-width: 0;
+ background-image:url('nav_g.png');
+ background-repeat:repeat-x;
+ background-color: #FFFFFF;
+ /* opera specific markup */
+ border-bottom-left-radius: 4px;
+ border-bottom-right-radius: 4px;
+ box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15);
+ /* firefox specific markup */
+ -moz-border-radius-bottomleft: 4px;
+ -moz-border-radius-bottomright: 4px;
+ -moz-box-shadow: rgba(0, 0, 0, 0.15) 5px 5px 5px;
+ /* webkit specific markup */
+ -webkit-border-bottom-left-radius: 4px;
+ -webkit-border-bottom-right-radius: 4px;
+ -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15);
+}
+
+dl.reflist dt {
+ padding: 5px;
+}
+
+dl.reflist dd {
+ margin: 0px 0px 10px 0px;
+ padding: 5px;
+}
+
+.paramkey {
+ text-align: right;
+}
+
+.paramtype {
+ white-space: nowrap;
+}
+
+.paramname {
+ color: #602020;
+ white-space: nowrap;
+}
+.paramname em {
+ font-style: normal;
+}
+.paramname code {
+ line-height: 14px;
+}
+
+.params, .retval, .exception, .tparams {
+ margin-left: 0px;
+ padding-left: 0px;
+}
+
+.params .paramname, .retval .paramname, .tparams .paramname, .exception .paramname {
+ font-weight: bold;
+ vertical-align: top;
+}
+
+.params .paramtype, .tparams .paramtype {
+ font-style: italic;
+ vertical-align: top;
+}
+
+.params .paramdir, .tparams .paramdir {
+ font-family: "courier new",courier,monospace;
+ vertical-align: top;
+}
+
+table.mlabels {
+ border-spacing: 0px;
+}
+
+td.mlabels-left {
+ width: 100%;
+ padding: 0px;
+}
+
+td.mlabels-right {
+ vertical-align: bottom;
+ padding: 0px;
+ white-space: nowrap;
+}
+
+span.mlabels {
+ margin-left: 8px;
+}
+
+span.mlabel {
+ background-color: #728DC1;
+ border-top:1px solid #5373B4;
+ border-left:1px solid #5373B4;
+ border-right:1px solid #C4CFE5;
+ border-bottom:1px solid #C4CFE5;
+ text-shadow: none;
+ color: white;
+ margin-right: 4px;
+ padding: 2px 3px;
+ border-radius: 3px;
+ font-size: 7pt;
+ white-space: nowrap;
+ vertical-align: middle;
+}
+
+
+
+/* @end */
+
+/* these are for tree view inside a (index) page */
+
+div.directory {
+ margin: 10px 0px;
+ border-top: 1px solid #9CAFD4;
+ border-bottom: 1px solid #9CAFD4;
+ width: 100%;
+}
+
+.directory table {
+ border-collapse:collapse;
+}
+
+.directory td {
+ margin: 0px;
+ padding: 0px;
+ vertical-align: top;
+}
+
+.directory td.entry {
+ white-space: nowrap;
+ padding-right: 6px;
+ padding-top: 3px;
+}
+
+.directory td.entry a {
+ outline:none;
+}
+
+.directory td.entry a img {
+ border: none;
+}
+
+.directory td.desc {
+ width: 100%;
+ padding-left: 6px;
+ padding-right: 6px;
+ padding-top: 3px;
+ border-left: 1px solid rgba(0,0,0,0.05);
+}
+
+.directory tr.even {
+ padding-left: 6px;
+ background-color: #F7F8FB;
+}
+
+.directory img {
+ vertical-align: -30%;
+}
+
+.directory .levels {
+ white-space: nowrap;
+ width: 100%;
+ text-align: right;
+ font-size: 9pt;
+}
+
+.directory .levels span {
+ cursor: pointer;
+ padding-left: 2px;
+ padding-right: 2px;
+ color: #3D578C;
+}
+
+.arrow {
+ color: #9CAFD4;
+ -webkit-user-select: none;
+ -khtml-user-select: none;
+ -moz-user-select: none;
+ -ms-user-select: none;
+ user-select: none;
+ cursor: pointer;
+ font-size: 80%;
+ display: inline-block;
+ width: 16px;
+ height: 22px;
+}
+
+.icon {
+ font-family: Arial, Helvetica;
+ font-weight: bold;
+ font-size: 12px;
+ height: 14px;
+ width: 16px;
+ display: inline-block;
+ background-color: #728DC1;
+ color: white;
+ text-align: center;
+ border-radius: 4px;
+ margin-left: 2px;
+ margin-right: 2px;
+}
+
+.icona {
+ width: 24px;
+ height: 22px;
+ display: inline-block;
+}
+
+.iconfopen {
+ width: 24px;
+ height: 18px;
+ margin-bottom: 4px;
+ background-image:url('folderopen.png');
+ background-position: 0px -4px;
+ background-repeat: repeat-y;
+ vertical-align:top;
+ display: inline-block;
+}
+
+.iconfclosed {
+ width: 24px;
+ height: 18px;
+ margin-bottom: 4px;
+ background-image:url('folderclosed.png');
+ background-position: 0px -4px;
+ background-repeat: repeat-y;
+ vertical-align:top;
+ display: inline-block;
+}
+
+.icondoc {
+ width: 24px;
+ height: 18px;
+ margin-bottom: 4px;
+ background-image:url('doc.png');
+ background-position: 0px -4px;
+ background-repeat: repeat-y;
+ vertical-align:top;
+ display: inline-block;
+}
+
+table.directory {
+ font: 400 14px Roboto,sans-serif;
+}
+
+/* @end */
+
+div.dynheader {
+ margin-top: 8px;
+ -webkit-touch-callout: none;
+ -webkit-user-select: none;
+ -khtml-user-select: none;
+ -moz-user-select: none;
+ -ms-user-select: none;
+ user-select: none;
+}
+
+address {
+ font-style: normal;
+ color: #2A3D61;
+}
+
+table.doxtable caption {
+ caption-side: top;
+}
+
+table.doxtable {
+ border-collapse:collapse;
+ margin-top: 4px;
+ margin-bottom: 4px;
+}
+
+table.doxtable td, table.doxtable th {
+ border: 1px solid #2D4068;
+ padding: 3px 7px 2px;
+}
+
+table.doxtable th {
+ background-color: #374F7F;
+ color: #FFFFFF;
+ font-size: 110%;
+ padding-bottom: 4px;
+ padding-top: 5px;
+}
+
+table.fieldtable {
+ /*width: 100%;*/
+ margin-bottom: 10px;
+ border: 1px solid #A8B8D9;
+ border-spacing: 0px;
+ -moz-border-radius: 4px;
+ -webkit-border-radius: 4px;
+ border-radius: 4px;
+ -moz-box-shadow: rgba(0, 0, 0, 0.15) 2px 2px 2px;
+ -webkit-box-shadow: 2px 2px 2px rgba(0, 0, 0, 0.15);
+ box-shadow: 2px 2px 2px rgba(0, 0, 0, 0.15);
+}
+
+.fieldtable td, .fieldtable th {
+ padding: 3px 7px 2px;
+}
+
+.fieldtable td.fieldtype, .fieldtable td.fieldname {
+ white-space: nowrap;
+ border-right: 1px solid #A8B8D9;
+ border-bottom: 1px solid #A8B8D9;
+ vertical-align: top;
+}
+
+.fieldtable td.fieldname {
+ padding-top: 3px;
+}
+
+.fieldtable td.fielddoc {
+ border-bottom: 1px solid #A8B8D9;
+ /*width: 100%;*/
+}
+
+.fieldtable td.fielddoc p:first-child {
+ margin-top: 0px;
+}
+
+.fieldtable td.fielddoc p:last-child {
+ margin-bottom: 2px;
+}
+
+.fieldtable tr:last-child td {
+ border-bottom: none;
+}
+
+.fieldtable th {
+ background-image:url('nav_f.png');
+ background-repeat:repeat-x;
+ background-color: #E2E8F2;
+ font-size: 90%;
+ color: #253555;
+ padding-bottom: 4px;
+ padding-top: 5px;
+ text-align:left;
+ font-weight: 400;
+ -moz-border-radius-topleft: 4px;
+ -moz-border-radius-topright: 4px;
+ -webkit-border-top-left-radius: 4px;
+ -webkit-border-top-right-radius: 4px;
+ border-top-left-radius: 4px;
+ border-top-right-radius: 4px;
+ border-bottom: 1px solid #A8B8D9;
+}
+
+
+.tabsearch {
+ top: 0px;
+ left: 10px;
+ height: 36px;
+ background-image: url('tab_b.png');
+ z-index: 101;
+ overflow: hidden;
+ font-size: 13px;
+}
+
+.navpath ul
+{
+ font-size: 11px;
+ background-image:url('tab_b.png');
+ background-repeat:repeat-x;
+ background-position: 0 -5px;
+ height:30px;
+ line-height:30px;
+ color:#8AA0CC;
+ border:solid 1px #C2CDE4;
+ overflow:hidden;
+ margin:0px;
+ padding:0px;
+}
+
+.navpath li
+{
+ list-style-type:none;
+ float:left;
+ padding-left:10px;
+ padding-right:15px;
+ background-image:url('bc_s.png');
+ background-repeat:no-repeat;
+ background-position:right;
+ color:#364D7C;
+}
+
+.navpath li.navelem a
+{
+ height:32px;
+ display:block;
+ text-decoration: none;
+ outline: none;
+ color: #283A5D;
+ font-family: 'Lucida Grande',Geneva,Helvetica,Arial,sans-serif;
+ text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9);
+ text-decoration: none;
+}
+
+.navpath li.navelem a:hover
+{
+ color:#6884BD;
+}
+
+.navpath li.footer
+{
+ list-style-type:none;
+ float:right;
+ padding-left:10px;
+ padding-right:15px;
+ background-image:none;
+ background-repeat:no-repeat;
+ background-position:right;
+ color:#364D7C;
+ font-size: 8pt;
+}
+
+
+div.summary
+{
+ float: right;
+ font-size: 8pt;
+ padding-right: 5px;
+ width: 50%;
+ text-align: right;
+}
+
+div.summary a
+{
+ white-space: nowrap;
+}
+
+table.classindex
+{
+ margin: 10px;
+ white-space: nowrap;
+ margin-left: 3%;
+ margin-right: 3%;
+ width: 94%;
+ border: 0;
+ border-spacing: 0;
+ padding: 0;
+}
+
+div.ingroups
+{
+ font-size: 8pt;
+ width: 50%;
+ text-align: left;
+}
+
+div.ingroups a
+{
+ white-space: nowrap;
+}
+
+div.header
+{
+ background-image:url('nav_h.png');
+ background-repeat:repeat-x;
+ background-color: #F9FAFC;
+ margin: 0px;
+ border-bottom: 1px solid #C4CFE5;
+}
+
+div.headertitle
+{
+ padding: 5px 5px 5px 10px;
+}
+
+.PageDocRTL-title div.headertitle {
+ text-align: right;
+ direction: rtl;
+}
+
+dl {
+ padding: 0 0 0 0;
+}
+
+/* dl.note, dl.warning, dl.attention, dl.pre, dl.post, dl.invariant, dl.deprecated, dl.todo, dl.test, dl.bug, dl.examples */
+dl.section {
+ margin-left: 0px;
+ padding-left: 0px;
+}
+
+dl.section.DocNodeRTL {
+ margin-right: 0px;
+ padding-right: 0px;
+}
+
+dl.note {
+ margin-left: -7px;
+ padding-left: 3px;
+ border-left: 4px solid;
+ border-color: #D0C000;
+}
+
+dl.note.DocNodeRTL {
+ margin-left: 0;
+ padding-left: 0;
+ border-left: 0;
+ margin-right: -7px;
+ padding-right: 3px;
+ border-right: 4px solid;
+ border-color: #D0C000;
+}
+
+dl.warning, dl.attention {
+ margin-left: -7px;
+ padding-left: 3px;
+ border-left: 4px solid;
+ border-color: #FF0000;
+}
+
+dl.warning.DocNodeRTL, dl.attention.DocNodeRTL {
+ margin-left: 0;
+ padding-left: 0;
+ border-left: 0;
+ margin-right: -7px;
+ padding-right: 3px;
+ border-right: 4px solid;
+ border-color: #FF0000;
+}
+
+dl.pre, dl.post, dl.invariant {
+ margin-left: -7px;
+ padding-left: 3px;
+ border-left: 4px solid;
+ border-color: #00D000;
+}
+
+dl.pre.DocNodeRTL, dl.post.DocNodeRTL, dl.invariant.DocNodeRTL {
+ margin-left: 0;
+ padding-left: 0;
+ border-left: 0;
+ margin-right: -7px;
+ padding-right: 3px;
+ border-right: 4px solid;
+ border-color: #00D000;
+}
+
+dl.deprecated {
+ margin-left: -7px;
+ padding-left: 3px;
+ border-left: 4px solid;
+ border-color: #505050;
+}
+
+dl.deprecated.DocNodeRTL {
+ margin-left: 0;
+ padding-left: 0;
+ border-left: 0;
+ margin-right: -7px;
+ padding-right: 3px;
+ border-right: 4px solid;
+ border-color: #505050;
+}
+
+dl.todo {
+ margin-left: -7px;
+ padding-left: 3px;
+ border-left: 4px solid;
+ border-color: #00C0E0;
+}
+
+dl.todo.DocNodeRTL {
+ margin-left: 0;
+ padding-left: 0;
+ border-left: 0;
+ margin-right: -7px;
+ padding-right: 3px;
+ border-right: 4px solid;
+ border-color: #00C0E0;
+}
+
+dl.test {
+ margin-left: -7px;
+ padding-left: 3px;
+ border-left: 4px solid;
+ border-color: #3030E0;
+}
+
+dl.test.DocNodeRTL {
+ margin-left: 0;
+ padding-left: 0;
+ border-left: 0;
+ margin-right: -7px;
+ padding-right: 3px;
+ border-right: 4px solid;
+ border-color: #3030E0;
+}
+
+dl.bug {
+ margin-left: -7px;
+ padding-left: 3px;
+ border-left: 4px solid;
+ border-color: #C08050;
+}
+
+dl.bug.DocNodeRTL {
+ margin-left: 0;
+ padding-left: 0;
+ border-left: 0;
+ margin-right: -7px;
+ padding-right: 3px;
+ border-right: 4px solid;
+ border-color: #C08050;
+}
+
+dl.section dd {
+ margin-bottom: 6px;
+}
+
+
+#projectlogo
+{
+ text-align: center;
+ vertical-align: bottom;
+ border-collapse: separate;
+}
+
+#projectlogo img
+{
+ border: 0px none;
+}
+
+#projectalign
+{
+ vertical-align: middle;
+}
+
+#projectname
+{
+ font: 300% Tahoma, Arial,sans-serif;
+ margin: 0px;
+ padding: 2px 0px;
+}
+
+#projectbrief
+{
+ font: 120% Tahoma, Arial,sans-serif;
+ margin: 0px;
+ padding: 0px;
+}
+
+#projectnumber
+{
+ font: 50% Tahoma, Arial,sans-serif;
+ margin: 0px;
+ padding: 0px;
+}
+
+#titlearea
+{
+ padding: 0px;
+ margin: 0px;
+ width: 100%;
+ border-bottom: 1px solid #5373B4;
+}
+
+.image
+{
+ text-align: center;
+}
+
+.dotgraph
+{
+ text-align: center;
+}
+
+.mscgraph
+{
+ text-align: center;
+}
+
+.plantumlgraph
+{
+ text-align: center;
+}
+
+.diagraph
+{
+ text-align: center;
+}
+
+.caption
+{
+ font-weight: bold;
+}
+
+div.zoom
+{
+ border: 1px solid #90A5CE;
+}
+
+dl.citelist {
+ margin-bottom:50px;
+}
+
+dl.citelist dt {
+ color:#334975;
+ float:left;
+ font-weight:bold;
+ margin-right:10px;
+ padding:5px;
+ text-align:right;
+ width:52px;
+}
+
+dl.citelist dd {
+ margin:2px 0 2px 72px;
+ padding:5px 0;
+}
+
+div.toc {
+ padding: 14px 25px;
+ background-color: #F4F6FA;
+ border: 1px solid #D8DFEE;
+ border-radius: 7px 7px 7px 7px;
+ float: right;
+ height: auto;
+ margin: 0 8px 10px 10px;
+ width: 200px;
+}
+
+.PageDocRTL-title div.toc {
+ float: left !important;
+ text-align: right;
+}
+
+div.toc li {
+ background: url("bdwn.png") no-repeat scroll 0 5px transparent;
+ font: 10px/1.2 Verdana,DejaVu Sans,Geneva,sans-serif;
+ margin-top: 5px;
+ padding-left: 10px;
+ padding-top: 2px;
+}
+
+.PageDocRTL-title div.toc li {
+ background-position-x: right !important;
+ padding-left: 0 !important;
+ padding-right: 10px;
+}
+
+div.toc h3 {
+ font: bold 12px/1.2 Arial,FreeSans,sans-serif;
+ color: #4665A2;
+ border-bottom: 0 none;
+ margin: 0;
+}
+
+div.toc ul {
+ list-style: none outside none;
+ border: medium none;
+ padding: 0px;
+}
+
+div.toc li.level1 {
+ margin-left: 0px;
+}
+
+div.toc li.level2 {
+ margin-left: 15px;
+}
+
+div.toc li.level3 {
+ margin-left: 30px;
+}
+
+div.toc li.level4 {
+ margin-left: 45px;
+}
+
+span.emoji {
+ /* font family used at the site: https://unicode.org/emoji/charts/full-emoji-list.html
+ * font-family: "Noto Color Emoji", "Apple Color Emoji", "Segoe UI Emoji", Times, Symbola, Aegyptus, Code2000, Code2001, Code2002, Musica, serif, LastResort;
+ */
+}
+
+.PageDocRTL-title div.toc li.level1 {
+ margin-left: 0 !important;
+ margin-right: 0;
+}
+
+.PageDocRTL-title div.toc li.level2 {
+ margin-left: 0 !important;
+ margin-right: 15px;
+}
+
+.PageDocRTL-title div.toc li.level3 {
+ margin-left: 0 !important;
+ margin-right: 30px;
+}
+
+.PageDocRTL-title div.toc li.level4 {
+ margin-left: 0 !important;
+ margin-right: 45px;
+}
+
+.inherit_header {
+ font-weight: bold;
+ color: gray;
+ cursor: pointer;
+ -webkit-touch-callout: none;
+ -webkit-user-select: none;
+ -khtml-user-select: none;
+ -moz-user-select: none;
+ -ms-user-select: none;
+ user-select: none;
+}
+
+.inherit_header td {
+ padding: 6px 0px 2px 5px;
+}
+
+.inherit {
+ display: none;
+}
+
+tr.heading h2 {
+ margin-top: 12px;
+ margin-bottom: 4px;
+}
+
+/* tooltip related style info */
+
+.ttc {
+ position: absolute;
+ display: none;
+}
+
+#powerTip {
+ cursor: default;
+ white-space: nowrap;
+ background-color: white;
+ border: 1px solid gray;
+ border-radius: 4px 4px 4px 4px;
+ box-shadow: 1px 1px 7px gray;
+ display: none;
+ font-size: smaller;
+ max-width: 80%;
+ opacity: 0.9;
+ padding: 1ex 1em 1em;
+ position: absolute;
+ z-index: 2147483647;
+}
+
+#powerTip div.ttdoc {
+ color: grey;
+ font-style: italic;
+}
+
+#powerTip div.ttname a {
+ font-weight: bold;
+}
+
+#powerTip div.ttname {
+ font-weight: bold;
+}
+
+#powerTip div.ttdeci {
+ color: #006318;
+}
+
+#powerTip div {
+ margin: 0px;
+ padding: 0px;
+ font: 12px/16px Roboto,sans-serif;
+}
+
+#powerTip:before, #powerTip:after {
+ content: "";
+ position: absolute;
+ margin: 0px;
+}
+
+#powerTip.n:after, #powerTip.n:before,
+#powerTip.s:after, #powerTip.s:before,
+#powerTip.w:after, #powerTip.w:before,
+#powerTip.e:after, #powerTip.e:before,
+#powerTip.ne:after, #powerTip.ne:before,
+#powerTip.se:after, #powerTip.se:before,
+#powerTip.nw:after, #powerTip.nw:before,
+#powerTip.sw:after, #powerTip.sw:before {
+ border: solid transparent;
+ content: " ";
+ height: 0;
+ width: 0;
+ position: absolute;
+}
+
+#powerTip.n:after, #powerTip.s:after,
+#powerTip.w:after, #powerTip.e:after,
+#powerTip.nw:after, #powerTip.ne:after,
+#powerTip.sw:after, #powerTip.se:after {
+ border-color: rgba(255, 255, 255, 0);
+}
+
+#powerTip.n:before, #powerTip.s:before,
+#powerTip.w:before, #powerTip.e:before,
+#powerTip.nw:before, #powerTip.ne:before,
+#powerTip.sw:before, #powerTip.se:before {
+ border-color: rgba(128, 128, 128, 0);
+}
+
+#powerTip.n:after, #powerTip.n:before,
+#powerTip.ne:after, #powerTip.ne:before,
+#powerTip.nw:after, #powerTip.nw:before {
+ top: 100%;
+}
+
+#powerTip.n:after, #powerTip.ne:after, #powerTip.nw:after {
+ border-top-color: #FFFFFF;
+ border-width: 10px;
+ margin: 0px -10px;
+}
+#powerTip.n:before {
+ border-top-color: #808080;
+ border-width: 11px;
+ margin: 0px -11px;
+}
+#powerTip.n:after, #powerTip.n:before {
+ left: 50%;
+}
+
+#powerTip.nw:after, #powerTip.nw:before {
+ right: 14px;
+}
+
+#powerTip.ne:after, #powerTip.ne:before {
+ left: 14px;
+}
+
+#powerTip.s:after, #powerTip.s:before,
+#powerTip.se:after, #powerTip.se:before,
+#powerTip.sw:after, #powerTip.sw:before {
+ bottom: 100%;
+}
+
+#powerTip.s:after, #powerTip.se:after, #powerTip.sw:after {
+ border-bottom-color: #FFFFFF;
+ border-width: 10px;
+ margin: 0px -10px;
+}
+
+#powerTip.s:before, #powerTip.se:before, #powerTip.sw:before {
+ border-bottom-color: #808080;
+ border-width: 11px;
+ margin: 0px -11px;
+}
+
+#powerTip.s:after, #powerTip.s:before {
+ left: 50%;
+}
+
+#powerTip.sw:after, #powerTip.sw:before {
+ right: 14px;
+}
+
+#powerTip.se:after, #powerTip.se:before {
+ left: 14px;
+}
+
+#powerTip.e:after, #powerTip.e:before {
+ left: 100%;
+}
+#powerTip.e:after {
+ border-left-color: #FFFFFF;
+ border-width: 10px;
+ top: 50%;
+ margin-top: -10px;
+}
+#powerTip.e:before {
+ border-left-color: #808080;
+ border-width: 11px;
+ top: 50%;
+ margin-top: -11px;
+}
+
+#powerTip.w:after, #powerTip.w:before {
+ right: 100%;
+}
+#powerTip.w:after {
+ border-right-color: #FFFFFF;
+ border-width: 10px;
+ top: 50%;
+ margin-top: -10px;
+}
+#powerTip.w:before {
+ border-right-color: #808080;
+ border-width: 11px;
+ top: 50%;
+ margin-top: -11px;
+}
+
+@media print
+{
+ #top { display: none; }
+ #side-nav { display: none; }
+ #nav-path { display: none; }
+ body { overflow:visible; }
+ h1, h2, h3, h4, h5, h6 { page-break-after: avoid; }
+ .summary { display: none; }
+ .memitem { page-break-inside: avoid; }
+ #doc-content
+ {
+ margin-left:0 !important;
+ height:auto !important;
+ width:auto !important;
+ overflow:inherit;
+ display:inline;
+ }
+}
+
+/* @group Markdown */
+
+table.markdownTable {
+ border-collapse:collapse;
+ margin-top: 4px;
+ margin-bottom: 4px;
+}
+
+table.markdownTable td, table.markdownTable th {
+ border: 1px solid #2D4068;
+ padding: 3px 7px 2px;
+}
+
+table.markdownTable tr {
+}
+
+th.markdownTableHeadLeft, th.markdownTableHeadRight, th.markdownTableHeadCenter, th.markdownTableHeadNone {
+ background-color: #374F7F;
+ color: #FFFFFF;
+ font-size: 110%;
+ padding-bottom: 4px;
+ padding-top: 5px;
+}
+
+th.markdownTableHeadLeft, td.markdownTableBodyLeft {
+ text-align: left
+}
+
+th.markdownTableHeadRight, td.markdownTableBodyRight {
+ text-align: right
+}
+
+th.markdownTableHeadCenter, td.markdownTableBodyCenter {
+ text-align: center
+}
+
+.DocNodeRTL {
+ text-align: right;
+ direction: rtl;
+}
+
+.DocNodeLTR {
+ text-align: left;
+ direction: ltr;
+}
+
+table.DocNodeRTL {
+ width: auto;
+ margin-right: 0;
+ margin-left: auto;
+}
+
+table.DocNodeLTR {
+ width: auto;
+ margin-right: auto;
+ margin-left: 0;
+}
+
+tt, code, kbd, samp
+{
+ display: inline-block;
+ direction:ltr;
+}
+/* @end */
+
+u {
+ text-decoration: underline;
+}
+
diff --git a/doxygen.svg b/doxygen.svg
new file mode 100644
index 000000000..d42dad52d
--- /dev/null
+++ b/doxygen.svg
@@ -0,0 +1,26 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/dynsections.js b/dynsections.js
new file mode 100644
index 000000000..3174bd7be
--- /dev/null
+++ b/dynsections.js
@@ -0,0 +1,121 @@
+/*
+ @licstart The following is the entire license notice for the JavaScript code in this file.
+
+ The MIT License (MIT)
+
+ Copyright (C) 1997-2020 by Dimitri van Heesch
+
+ Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ and associated documentation files (the "Software"), to deal in the Software without restriction,
+ including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
+ furnished to do so, subject to the following conditions:
+
+ The above copyright notice and this permission notice shall be included in all copies or
+ substantial portions of the Software.
+
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+ @licend The above is the entire license notice for the JavaScript code in this file
+ */
+function toggleVisibility(linkObj)
+{
+ var base = $(linkObj).attr('id');
+ var summary = $('#'+base+'-summary');
+ var content = $('#'+base+'-content');
+ var trigger = $('#'+base+'-trigger');
+ var src=$(trigger).attr('src');
+ if (content.is(':visible')===true) {
+ content.hide();
+ summary.show();
+ $(linkObj).addClass('closed').removeClass('opened');
+ $(trigger).attr('src',src.substring(0,src.length-8)+'closed.png');
+ } else {
+ content.show();
+ summary.hide();
+ $(linkObj).removeClass('closed').addClass('opened');
+ $(trigger).attr('src',src.substring(0,src.length-10)+'open.png');
+ }
+ return false;
+}
+
+function updateStripes()
+{
+ $('table.directory tr').
+ removeClass('even').filter(':visible:even').addClass('even');
+}
+
+function toggleLevel(level)
+{
+ $('table.directory tr').each(function() {
+ var l = this.id.split('_').length-1;
+ var i = $('#img'+this.id.substring(3));
+ var a = $('#arr'+this.id.substring(3));
+ if (l
+
+
+
+
+
+
+kinematics-dynamics: File List
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented files with brief descriptions:
+
+
+
+
+
diff --git a/folderclosed.png b/folderclosed.png
new file mode 100644
index 000000000..bb8ab35ed
Binary files /dev/null and b/folderclosed.png differ
diff --git a/folderopen.png b/folderopen.png
new file mode 100644
index 000000000..d6c7f676a
Binary files /dev/null and b/folderopen.png differ
diff --git a/functions.html b/functions.html
new file mode 100644
index 000000000..9ed925f2d
--- /dev/null
+++ b/functions.html
@@ -0,0 +1,130 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- a -
+acceptBottle()
+: roboticslab::CentroidTransform
+
+acquireData()
+: roboticslab::InvalidDevice
+, roboticslab::LeapMotionSensorDevice
+, roboticslab::SpnavSensorDevice
+, roboticslab::StreamingDevice
+, roboticslab::WiimoteSensorDevice
+
+acquireInterfaces()
+: roboticslab::InvalidDevice
+, roboticslab::LeapMotionSensorDevice
+, roboticslab::SpnavSensorDevice
+, roboticslab::StreamingDevice
+, roboticslab::WiimoteSensorDevice
+
+act()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+append()
+: roboticslab::PoeExpression
+
+appendLink()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+applyConstraints()
+: roboticslab::ConfigurationSelectorHumanoidGait
+
+asFrame()
+: roboticslab::MatrixExponential
+
+AsibotConfiguration()
+: roboticslab::AsibotConfiguration
+
+AsibotConfigurationFactory()
+: roboticslab::AsibotConfigurationFactory
+
+AsibotConfigurationLeastOverallAngularDisplacement()
+: roboticslab::AsibotConfigurationLeastOverallAngularDisplacement
+
+AsibotConfigurationLeastOverallAngularDisplacementFactory()
+: roboticslab::AsibotConfigurationLeastOverallAngularDisplacementFactory
+
+
+
+
+
+
+
diff --git a/functions_b.html b/functions_b.html
new file mode 100644
index 000000000..9819d17be
--- /dev/null
+++ b/functions_b.html
@@ -0,0 +1,87 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- b -
+
+
+
+
+
diff --git a/functions_c.html b/functions_c.html
new file mode 100644
index 000000000..b14851ebc
--- /dev/null
+++ b/functions_c.html
@@ -0,0 +1,154 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- c -
+CartToJnt()
+: roboticslab::ChainIkSolverPos_ID
+, roboticslab::ChainIkSolverPos_ST
+
+CentroidTransform()
+: roboticslab::CentroidTransform
+
+chain
+: roboticslab::KdlSolver
+
+ChainIkSolverPos_ID()
+: roboticslab::ChainIkSolverPos_ID
+
+changeBase()
+: roboticslab::MatrixExponential
+
+changeBaseFrame()
+: roboticslab::PoeExpression
+
+changeOrigin()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+changeToolFrame()
+: roboticslab::PoeExpression
+
+checkJointsInLimits()
+: roboticslab::AsibotConfiguration::Pose
+
+cloneWithBase()
+: roboticslab::MatrixExponential
+
+Configuration()
+: roboticslab::ConfigurationSelector::Configuration
+
+ConfigurationSelector()
+: roboticslab::ConfigurationSelector
+
+ConfigurationSelectorFactory()
+: roboticslab::ConfigurationSelectorFactory
+
+ConfigurationSelectorHumanoidGait()
+: roboticslab::ConfigurationSelectorHumanoidGait
+
+ConfigurationSelectorHumanoidGaitFactory()
+: roboticslab::ConfigurationSelectorHumanoidGaitFactory
+
+ConfigurationSelectorLeastOverallAngularDisplacement()
+: roboticslab::ConfigurationSelectorLeastOverallAngularDisplacement
+
+ConfigurationSelectorLeastOverallAngularDisplacementFactory()
+: roboticslab::ConfigurationSelectorLeastOverallAngularDisplacementFactory
+
+configure()
+: OrderThreeTraj
+, roboticslab::AsibotConfiguration
+, roboticslab::ConfigurationSelector
+
+configureFixedAxes()
+: roboticslab::StreamingDevice
+
+create()
+: roboticslab::AsibotConfigurationFactory
+, roboticslab::AsibotConfigurationLeastOverallAngularDisplacementFactory
+, roboticslab::ChainFkSolverPos_ST
+, roboticslab::ChainIkSolverPos_ST
+, roboticslab::ConfigurationSelectorFactory
+, roboticslab::ConfigurationSelectorHumanoidGaitFactory
+, roboticslab::ConfigurationSelectorLeastOverallAngularDisplacementFactory
+, roboticslab::ScrewTheoryIkProblem
+
+
+
+
+
+
+
diff --git a/functions_d.html b/functions_d.html
new file mode 100644
index 000000000..72cd438ee
--- /dev/null
+++ b/functions_d.html
@@ -0,0 +1,100 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- d -
+describe()
+: roboticslab::PadenKahanOne
+, roboticslab::PadenKahanThree
+, roboticslab::PadenKahanTwo
+, roboticslab::PardosGotorFour
+, roboticslab::PardosGotorOne
+, roboticslab::PardosGotorThree
+, roboticslab::PardosGotorTwo
+, roboticslab::ScrewTheoryIkSubproblem
+
+diffInvKin()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+dump()
+: OrderThreeTraj
+
+
+
+
+
+
+
diff --git a/functions_e.html b/functions_e.html
new file mode 100644
index 000000000..1e8b0365a
--- /dev/null
+++ b/functions_e.html
@@ -0,0 +1,111 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- e -
+
+
+
+
+
diff --git a/functions_enum.html b/functions_enum.html
new file mode 100644
index 000000000..e01b2684b
--- /dev/null
+++ b/functions_enum.html
@@ -0,0 +1,91 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Enumerations
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_eval.html b/functions_eval.html
new file mode 100644
index 000000000..aab28933b
--- /dev/null
+++ b/functions_eval.html
@@ -0,0 +1,91 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Enumerator
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_f.html b/functions_f.html
new file mode 100644
index 000000000..05ab7fd19
--- /dev/null
+++ b/functions_f.html
@@ -0,0 +1,105 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- f -
+
+
+
+
+
diff --git a/functions_func.html b/functions_func.html
new file mode 100644
index 000000000..051c92e04
--- /dev/null
+++ b/functions_func.html
@@ -0,0 +1,130 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
- a -
+acceptBottle()
+: roboticslab::CentroidTransform
+
+acquireData()
+: roboticslab::InvalidDevice
+, roboticslab::LeapMotionSensorDevice
+, roboticslab::SpnavSensorDevice
+, roboticslab::StreamingDevice
+, roboticslab::WiimoteSensorDevice
+
+acquireInterfaces()
+: roboticslab::InvalidDevice
+, roboticslab::LeapMotionSensorDevice
+, roboticslab::SpnavSensorDevice
+, roboticslab::StreamingDevice
+, roboticslab::WiimoteSensorDevice
+
+act()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+append()
+: roboticslab::PoeExpression
+
+appendLink()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+applyConstraints()
+: roboticslab::ConfigurationSelectorHumanoidGait
+
+asFrame()
+: roboticslab::MatrixExponential
+
+AsibotConfiguration()
+: roboticslab::AsibotConfiguration
+
+AsibotConfigurationFactory()
+: roboticslab::AsibotConfigurationFactory
+
+AsibotConfigurationLeastOverallAngularDisplacement()
+: roboticslab::AsibotConfigurationLeastOverallAngularDisplacement
+
+AsibotConfigurationLeastOverallAngularDisplacementFactory()
+: roboticslab::AsibotConfigurationLeastOverallAngularDisplacementFactory
+
+
+
+
+
+
+
diff --git a/functions_func_b.html b/functions_func_b.html
new file mode 100644
index 000000000..e74777946
--- /dev/null
+++ b/functions_func_b.html
@@ -0,0 +1,84 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_func_c.html b/functions_func_c.html
new file mode 100644
index 000000000..10b9a13db
--- /dev/null
+++ b/functions_func_c.html
@@ -0,0 +1,151 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
- c -
+CartToJnt()
+: roboticslab::ChainIkSolverPos_ID
+, roboticslab::ChainIkSolverPos_ST
+
+CentroidTransform()
+: roboticslab::CentroidTransform
+
+ChainIkSolverPos_ID()
+: roboticslab::ChainIkSolverPos_ID
+
+changeBase()
+: roboticslab::MatrixExponential
+
+changeBaseFrame()
+: roboticslab::PoeExpression
+
+changeOrigin()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+changeToolFrame()
+: roboticslab::PoeExpression
+
+checkJointsInLimits()
+: roboticslab::AsibotConfiguration::Pose
+
+cloneWithBase()
+: roboticslab::MatrixExponential
+
+Configuration()
+: roboticslab::ConfigurationSelector::Configuration
+
+ConfigurationSelector()
+: roboticslab::ConfigurationSelector
+
+ConfigurationSelectorFactory()
+: roboticslab::ConfigurationSelectorFactory
+
+ConfigurationSelectorHumanoidGait()
+: roboticslab::ConfigurationSelectorHumanoidGait
+
+ConfigurationSelectorHumanoidGaitFactory()
+: roboticslab::ConfigurationSelectorHumanoidGaitFactory
+
+ConfigurationSelectorLeastOverallAngularDisplacement()
+: roboticslab::ConfigurationSelectorLeastOverallAngularDisplacement
+
+ConfigurationSelectorLeastOverallAngularDisplacementFactory()
+: roboticslab::ConfigurationSelectorLeastOverallAngularDisplacementFactory
+
+configure()
+: OrderThreeTraj
+, roboticslab::AsibotConfiguration
+, roboticslab::ConfigurationSelector
+
+configureFixedAxes()
+: roboticslab::StreamingDevice
+
+create()
+: roboticslab::AsibotConfigurationFactory
+, roboticslab::AsibotConfigurationLeastOverallAngularDisplacementFactory
+, roboticslab::ChainFkSolverPos_ST
+, roboticslab::ChainIkSolverPos_ST
+, roboticslab::ConfigurationSelectorFactory
+, roboticslab::ConfigurationSelectorHumanoidGaitFactory
+, roboticslab::ConfigurationSelectorLeastOverallAngularDisplacementFactory
+, roboticslab::ScrewTheoryIkProblem
+
+
+
+
+
+
+
diff --git a/functions_func_d.html b/functions_func_d.html
new file mode 100644
index 000000000..b7ee62696
--- /dev/null
+++ b/functions_func_d.html
@@ -0,0 +1,100 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
- d -
+describe()
+: roboticslab::PadenKahanOne
+, roboticslab::PadenKahanThree
+, roboticslab::PadenKahanTwo
+, roboticslab::PardosGotorFour
+, roboticslab::PardosGotorOne
+, roboticslab::PardosGotorThree
+, roboticslab::PardosGotorTwo
+, roboticslab::ScrewTheoryIkSubproblem
+
+diffInvKin()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+dump()
+: OrderThreeTraj
+
+
+
+
+
+
+
diff --git a/functions_func_e.html b/functions_func_e.html
new file mode 100644
index 000000000..20d749342
--- /dev/null
+++ b/functions_func_e.html
@@ -0,0 +1,87 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_func_f.html b/functions_func_f.html
new file mode 100644
index 000000000..266aacd2a
--- /dev/null
+++ b/functions_func_f.html
@@ -0,0 +1,102 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_func_g.html b/functions_func_g.html
new file mode 100644
index 000000000..a71121531
--- /dev/null
+++ b/functions_func_g.html
@@ -0,0 +1,144 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
- g -
+gcmp()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+get()
+: OrderThreeTraj
+
+getActuatorState()
+: roboticslab::LeapMotionSensorDevice
+, roboticslab::SpnavSensorDevice
+, roboticslab::StreamingDevice
+
+getAxis()
+: roboticslab::MatrixExponential
+
+getDiffs()
+: roboticslab::AsibotConfigurationLeastOverallAngularDisplacement
+, roboticslab::ConfigurationSelectorLeastOverallAngularDisplacement
+
+getdot()
+: OrderThreeTraj
+
+getdotdot()
+: OrderThreeTraj
+
+getMotionType()
+: roboticslab::MatrixExponential
+
+getNumJoints()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+getNumTcps()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+getOrigin()
+: roboticslab::MatrixExponential
+
+getParameter()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+getParameters()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+getSteps()
+: roboticslab::ScrewTheoryIkProblem
+
+getT()
+: OrderThreeTraj
+
+getTransform()
+: roboticslab::PoeExpression
+
+
+
+
+
+
+
diff --git a/functions_func_h.html b/functions_func_h.html
new file mode 100644
index 000000000..07fbeeee7
--- /dev/null
+++ b/functions_func_h.html
@@ -0,0 +1,86 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_func_i.html b/functions_func_i.html
new file mode 100644
index 000000000..10aa010ca
--- /dev/null
+++ b/functions_func_i.html
@@ -0,0 +1,116 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
- i -
+initialize()
+: roboticslab::LeapMotionSensorDevice
+, roboticslab::SpnavSensorDevice
+, roboticslab::StreamingDevice
+, roboticslab::WiimoteSensorDevice
+
+inv()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+invalidate()
+: roboticslab::ConfigurationSelector::Configuration
+
+InvalidDevice()
+: roboticslab::InvalidDevice
+
+invDyn()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+invKin()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+isReversed()
+: roboticslab::ScrewTheoryIkProblem
+
+isValid()
+: roboticslab::ConfigurationSelector::Configuration
+
+
+
+
+
+
+
diff --git a/functions_func_j.html b/functions_func_j.html
new file mode 100644
index 000000000..f6db7bcf6
--- /dev/null
+++ b/functions_func_j.html
@@ -0,0 +1,84 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_func_l.html b/functions_func_l.html
new file mode 100644
index 000000000..0c0c34097
--- /dev/null
+++ b/functions_func_l.html
@@ -0,0 +1,84 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_func_m.html b/functions_func_m.html
new file mode 100644
index 000000000..4a665fb9e
--- /dev/null
+++ b/functions_func_m.html
@@ -0,0 +1,114 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_func_p.html b/functions_func_p.html
new file mode 100644
index 000000000..081058a57
--- /dev/null
+++ b/functions_func_p.html
@@ -0,0 +1,122 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_func_r.html b/functions_func_r.html
new file mode 100644
index 000000000..4365151b6
--- /dev/null
+++ b/functions_func_r.html
@@ -0,0 +1,109 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_func_s.html b/functions_func_s.html
new file mode 100644
index 000000000..a2e2bba53
--- /dev/null
+++ b/functions_func_s.html
@@ -0,0 +1,169 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
- s -
+ScrewTheoryIkProblemBuilder()
+: roboticslab::ScrewTheoryIkProblemBuilder
+
+sendMovementCommand()
+: roboticslab::InvalidDevice
+, roboticslab::LeapMotionSensorDevice
+, roboticslab::SpnavSensorDevice
+, roboticslab::StreamingDevice
+, roboticslab::WiimoteSensorDevice
+
+setCartesianControllerHandle()
+: roboticslab::StreamingDevice
+
+setParameter()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+setParameters()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+setPermanenceTime()
+: roboticslab::CentroidTransform
+
+setTcpToCameraRotation()
+: roboticslab::CentroidTransform
+
+size()
+: roboticslab::PoeExpression
+
+solutions()
+: roboticslab::PadenKahanOne
+, roboticslab::PadenKahanThree
+, roboticslab::PadenKahanTwo
+, roboticslab::PardosGotorFour
+, roboticslab::PardosGotorOne
+, roboticslab::PardosGotorThree
+, roboticslab::PardosGotorTwo
+, roboticslab::ScrewTheoryIkProblem
+, roboticslab::ScrewTheoryIkSubproblem
+
+solve()
+: roboticslab::PadenKahanOne
+, roboticslab::PadenKahanThree
+, roboticslab::PadenKahanTwo
+, roboticslab::PardosGotorFour
+, roboticslab::PardosGotorOne
+, roboticslab::PardosGotorThree
+, roboticslab::PardosGotorTwo
+, roboticslab::ScrewTheoryIkProblem
+, roboticslab::ScrewTheoryIkSubproblem
+
+SpnavSensorDevice()
+: roboticslab::SpnavSensorDevice
+
+stat()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+stopControl()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+stopMotion()
+: roboticslab::InvalidDevice
+, roboticslab::LeapMotionSensorDevice
+, roboticslab::SpnavSensorDevice
+, roboticslab::StreamingDevice
+, roboticslab::WiimoteSensorDevice
+
+store()
+: roboticslab::ConfigurationSelector::Configuration
+
+storeAngles()
+: roboticslab::AsibotConfiguration::Pose
+
+StreamingDevice()
+: roboticslab::StreamingDevice
+
+strError()
+: roboticslab::ChainFkSolverPos_ST
+, roboticslab::ChainIkSolverPos_ID
+, roboticslab::ChainIkSolverPos_ST
+
+
+
+
+
+
+
diff --git a/functions_func_t.html b/functions_func_t.html
new file mode 100644
index 000000000..00403f2b7
--- /dev/null
+++ b/functions_func_t.html
@@ -0,0 +1,103 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_func_u.html b/functions_func_u.html
new file mode 100644
index 000000000..01b6db48a
--- /dev/null
+++ b/functions_func_u.html
@@ -0,0 +1,86 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_func_v.html b/functions_func_v.html
new file mode 100644
index 000000000..67550b55e
--- /dev/null
+++ b/functions_func_v.html
@@ -0,0 +1,84 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_func_w.html b/functions_func_w.html
new file mode 100644
index 000000000..dfd08f246
--- /dev/null
+++ b/functions_func_w.html
@@ -0,0 +1,94 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_func_~.html b/functions_func_~.html
new file mode 100644
index 000000000..674314ea1
--- /dev/null
+++ b/functions_func_~.html
@@ -0,0 +1,105 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Functions
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_g.html b/functions_g.html
new file mode 100644
index 000000000..2e7b4bafc
--- /dev/null
+++ b/functions_g.html
@@ -0,0 +1,144 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- g -
+gcmp()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+get()
+: OrderThreeTraj
+
+getActuatorState()
+: roboticslab::LeapMotionSensorDevice
+, roboticslab::SpnavSensorDevice
+, roboticslab::StreamingDevice
+
+getAxis()
+: roboticslab::MatrixExponential
+
+getDiffs()
+: roboticslab::AsibotConfigurationLeastOverallAngularDisplacement
+, roboticslab::ConfigurationSelectorLeastOverallAngularDisplacement
+
+getdot()
+: OrderThreeTraj
+
+getdotdot()
+: OrderThreeTraj
+
+getMotionType()
+: roboticslab::MatrixExponential
+
+getNumJoints()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+getNumTcps()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+getOrigin()
+: roboticslab::MatrixExponential
+
+getParameter()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+getParameters()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+getSteps()
+: roboticslab::ScrewTheoryIkProblem
+
+getT()
+: OrderThreeTraj
+
+getTransform()
+: roboticslab::PoeExpression
+
+
+
+
+
+
+
diff --git a/functions_h.html b/functions_h.html
new file mode 100644
index 000000000..b876a22f5
--- /dev/null
+++ b/functions_h.html
@@ -0,0 +1,86 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- h -
+
+
+
+
+
diff --git a/functions_i.html b/functions_i.html
new file mode 100644
index 000000000..fc8282f08
--- /dev/null
+++ b/functions_i.html
@@ -0,0 +1,116 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- i -
+initialize()
+: roboticslab::LeapMotionSensorDevice
+, roboticslab::SpnavSensorDevice
+, roboticslab::StreamingDevice
+, roboticslab::WiimoteSensorDevice
+
+inv()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+invalidate()
+: roboticslab::ConfigurationSelector::Configuration
+
+InvalidDevice()
+: roboticslab::InvalidDevice
+
+invDyn()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+invKin()
+: roboticslab::AsibotSolver
+, roboticslab::ICartesianSolver
+, roboticslab::KdlSolver
+, roboticslab::KdlTreeSolver
+
+isReversed()
+: roboticslab::ScrewTheoryIkProblem
+
+isValid()
+: roboticslab::ConfigurationSelector::Configuration
+
+
+
+
+
+
+
diff --git a/functions_j.html b/functions_j.html
new file mode 100644
index 000000000..4e34acdf2
--- /dev/null
+++ b/functions_j.html
@@ -0,0 +1,96 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- j -
+
+
+
+
+
diff --git a/functions_l.html b/functions_l.html
new file mode 100644
index 000000000..87e2ced1c
--- /dev/null
+++ b/functions_l.html
@@ -0,0 +1,84 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- l -
+
+
+
+
+
diff --git a/functions_m.html b/functions_m.html
new file mode 100644
index 000000000..a52037c0e
--- /dev/null
+++ b/functions_m.html
@@ -0,0 +1,120 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- m -
+
+
+
+
+
diff --git a/functions_o.html b/functions_o.html
new file mode 100644
index 000000000..321d8b1b7
--- /dev/null
+++ b/functions_o.html
@@ -0,0 +1,87 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- o -
+
+
+
+
+
diff --git a/functions_p.html b/functions_p.html
new file mode 100644
index 000000000..140182eb3
--- /dev/null
+++ b/functions_p.html
@@ -0,0 +1,122 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- p -
+
+
+
+
+
diff --git a/functions_r.html b/functions_r.html
new file mode 100644
index 000000000..70a1b982f
--- /dev/null
+++ b/functions_r.html
@@ -0,0 +1,115 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- r -
+
+
+
+
+
diff --git a/functions_s.html b/functions_s.html
new file mode 100644
index 000000000..bf7cffced
--- /dev/null
+++ b/functions_s.html
@@ -0,0 +1,178 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- s -
+ScrewTheoryIkProblemBuilder()
+: roboticslab::ScrewTheoryIkProblemBuilder
+
+sendMovementCommand()
+: roboticslab::InvalidDevice
+, roboticslab::LeapMotionSensorDevice
+, roboticslab::SpnavSensorDevice
+, roboticslab::StreamingDevice
+, roboticslab::WiimoteSensorDevice
+
+setCartesianControllerHandle()
+: roboticslab::StreamingDevice
+
+setParameter()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+setParameters()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+setPermanenceTime()
+: roboticslab::CentroidTransform
+
+setTcpToCameraRotation()
+: roboticslab::CentroidTransform
+
+size()
+: roboticslab::PoeExpression
+
+solutions()
+: roboticslab::PadenKahanOne
+, roboticslab::PadenKahanThree
+, roboticslab::PadenKahanTwo
+, roboticslab::PardosGotorFour
+, roboticslab::PardosGotorOne
+, roboticslab::PardosGotorThree
+, roboticslab::PardosGotorTwo
+, roboticslab::ScrewTheoryIkProblem
+
+Solutions
+: roboticslab::ScrewTheoryIkProblem
+, roboticslab::ScrewTheoryIkSubproblem
+
+solutions()
+: roboticslab::ScrewTheoryIkSubproblem
+
+solve()
+: roboticslab::PadenKahanOne
+, roboticslab::PadenKahanThree
+, roboticslab::PadenKahanTwo
+, roboticslab::PardosGotorFour
+, roboticslab::PardosGotorOne
+, roboticslab::PardosGotorThree
+, roboticslab::PardosGotorTwo
+, roboticslab::ScrewTheoryIkProblem
+, roboticslab::ScrewTheoryIkSubproblem
+
+SpnavSensorDevice()
+: roboticslab::SpnavSensorDevice
+
+stat()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+Steps
+: roboticslab::ScrewTheoryIkProblem
+
+stopControl()
+: roboticslab::BasicCartesianControl
+, roboticslab::CartesianControlClient
+, roboticslab::ICartesianControl
+
+stopMotion()
+: roboticslab::InvalidDevice
+, roboticslab::LeapMotionSensorDevice
+, roboticslab::SpnavSensorDevice
+, roboticslab::StreamingDevice
+, roboticslab::WiimoteSensorDevice
+
+store()
+: roboticslab::ConfigurationSelector::Configuration
+
+storeAngles()
+: roboticslab::AsibotConfiguration::Pose
+
+StreamingDevice()
+: roboticslab::StreamingDevice
+
+strError()
+: roboticslab::ChainFkSolverPos_ST
+, roboticslab::ChainIkSolverPos_ID
+, roboticslab::ChainIkSolverPos_ST
+
+
+
+
+
+
+
diff --git a/functions_t.html b/functions_t.html
new file mode 100644
index 000000000..b75eb2deb
--- /dev/null
+++ b/functions_t.html
@@ -0,0 +1,112 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- t -
+
+
+
+
+
diff --git a/functions_type.html b/functions_type.html
new file mode 100644
index 000000000..82556e703
--- /dev/null
+++ b/functions_type.html
@@ -0,0 +1,98 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Typedefs
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_u.html b/functions_u.html
new file mode 100644
index 000000000..e042edb5e
--- /dev/null
+++ b/functions_u.html
@@ -0,0 +1,86 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- u -
+
+
+
+
+
diff --git a/functions_v.html b/functions_v.html
new file mode 100644
index 000000000..c431ad855
--- /dev/null
+++ b/functions_v.html
@@ -0,0 +1,87 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- v -
+
+
+
+
+
diff --git a/functions_vars.html b/functions_vars.html
new file mode 100644
index 000000000..c65787825
--- /dev/null
+++ b/functions_vars.html
@@ -0,0 +1,118 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members - Variables
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/functions_w.html b/functions_w.html
new file mode 100644
index 000000000..1aae91d7f
--- /dev/null
+++ b/functions_w.html
@@ -0,0 +1,94 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- w -
+
+
+
+
+
diff --git a/functions_~.html b/functions_~.html
new file mode 100644
index 000000000..743ea08b8
--- /dev/null
+++ b/functions_~.html
@@ -0,0 +1,105 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented class members with links to the class documentation for each member:
+
+
- ~ -
+
+
+
+
+
diff --git a/globals.html b/globals.html
new file mode 100644
index 000000000..3d9017457
--- /dev/null
+++ b/globals.html
@@ -0,0 +1,195 @@
+
+
+
+
+
+
+
+kinematics-dynamics: File Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Here is a list of all documented file members with links to the documentation:
+
+
- v -
+
+
+
+
+
diff --git a/globals_vars.html b/globals_vars.html
new file mode 100644
index 000000000..df72a23a8
--- /dev/null
+++ b/globals_vars.html
@@ -0,0 +1,195 @@
+
+
+
+
+
+
+
+kinematics-dynamics: File Members
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/group__AsibotSolver.html b/group__AsibotSolver.html
new file mode 100644
index 000000000..86119cc88
--- /dev/null
+++ b/group__AsibotSolver.html
@@ -0,0 +1,106 @@
+
+
+
+
+
+
+
+kinematics-dynamics: AsibotSolver
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/group__BasicCartesianControl.html b/group__BasicCartesianControl.html
new file mode 100644
index 000000000..f3b854b32
--- /dev/null
+++ b/group__BasicCartesianControl.html
@@ -0,0 +1,122 @@
+
+
+
+
+
+
+
+kinematics-dynamics: BasicCartesianControl
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Contains roboticslab::BasicCartesianControl .
+More...
+
+
+
+Example with a Fake robot
+
First we must run a YARP name server if it is not running in our current namespace:
+
[on terminal 1] yarp server
+ And then launch the actual library:
+
[on terminal 2] yarpdev --device BasicCartesianControl --robot EmulatedControlBoard --angleRepr axisAngle --link_0 "(A 1)"
+ Along the output, observe a line like the following.
[info] DeviceDriverImpl.cpp:26 open(): Using angleRepr: axisAngle.
+ This would mean we are using an axis/angle notation (par de rotación). Note that it is a variable parameter; in fact, we have forced it above.
+
We connect, can ask for help, etc. Here's an example interaction:
[on terminal 3] yarp rpc /CartesianControl/rpc_transform:s
+[>>] help
+Response: [stat] [inv] [movj] [movl] [movv] [gcmp] [forc] [stop]
+[>>] stat
+Response: [ccnc] 1.0 0.0 0.0 0.0 0.0 1.0 0.0
+ Stat returns the controller status, and forward kinematics. [ccnc] means Cartesian Control Not Controlling (also note that notation is in constant evolution, so put an issue if it's not updated!). The first 3 parameters of the forward kinnematics are X,Y,Z of the end-effector. The other parameters correspond to orientation in the previously set notation, which in this example is axis/angle, so this would read: on Z, 0 degrees.
+
Let's now move to a reachable position/orientation.
[>>] movj 1.0 0.0 0.0 0.0 0.0 1.0 0.0
+Response: [ok]
+
+Example with teoSim
+
What moslty changes is the library command line invocation. We also change the server port name. The following is an example for the simulated robot's right arm.
[on terminal 2] yarpdev --device BasicCartesianControl --name /teoSim/rightArm/CartesianControl --from /usr/local/share/teo-configuration-files/contexts/kinematics/rightArmKinematics.ini --angleRepr axisAngle --robot remote_controlboard --local /BasicCartesianControl/teoSim/rightArm --remote /teoSim/rightArm
+[on terminal 3] yarp rpc /teoSim/rightArm/CartesianControl/rpc_transform:s
+
+Example with real TEO
+
What moslty changes is the library command line invocation. We also change the server port name. The following is an example for the robot's right arm.
[on terminal 2] yarpdev --device BasicCartesianControl --name /teo/rightArm/CartesianControl --from /usr/local/share/teo-configuration-files/contexts/kinematics/rightArmKinematics.ini --angleRepr axisAngle --robot remote_controlboard --local /BasicCartesianControl/teo/rightArm --remote /teo/rightArm
+[on terminal 3] yarp rpc /teo/rightArm/CartesianControl/rpc:s
+ On the real robot, you can even activate Gravity Compensation (warning: dangerous if kinematic/dynamic model has not been reviewed!).
[>>] gcmp
+
+Very Important
+
When you launch the BasicCartesianControl device as in [terminal 2], it's actually wrapped: CartesianControlServer is the device that is actually loaded, and BasicCartesianControl becomes its subdevice. The server is what allows us to interact via the YARP RPC port mechanism. If you are creating a C++ program, you do not have to sned these raw port commands. Use CartesianControlClient instead, because the server and client are YARP devices (further reading on why this is good: here (spanish) ).
+
+
+
+
+
diff --git a/group__CartesianControlClient.html b/group__CartesianControlClient.html
new file mode 100644
index 000000000..0493e5ebd
--- /dev/null
+++ b/group__CartesianControlClient.html
@@ -0,0 +1,97 @@
+
+
+
+
+
+
+
+kinematics-dynamics: CartesianControlClient
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/group__CartesianControlServer.html b/group__CartesianControlServer.html
new file mode 100644
index 000000000..2101d8fd0
--- /dev/null
+++ b/group__CartesianControlServer.html
@@ -0,0 +1,103 @@
+
+
+
+
+
+
+
+kinematics-dynamics: CartesianControlServer
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/group__KdlSolver.html b/group__KdlSolver.html
new file mode 100644
index 000000000..eb05a242c
--- /dev/null
+++ b/group__KdlSolver.html
@@ -0,0 +1,103 @@
+
+
+
+
+
+
+
+kinematics-dynamics: KdlSolver
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/group__KdlTreeSolver.html b/group__KdlTreeSolver.html
new file mode 100644
index 000000000..5d72f87ae
--- /dev/null
+++ b/group__KdlTreeSolver.html
@@ -0,0 +1,94 @@
+
+
+
+
+
+
+
+kinematics-dynamics: KdlTreeSolver
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/group__KdlVectorConverterLib.html b/group__KdlVectorConverterLib.html
new file mode 100644
index 000000000..48bc007cb
--- /dev/null
+++ b/group__KdlVectorConverterLib.html
@@ -0,0 +1,84 @@
+
+
+
+
+
+
+
+kinematics-dynamics: KdlVectorConverterLib
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Contains classes related to KDL and std::vector classes.
+
+
+
+
+
+
diff --git a/group__KinematicRepresentationLib.html b/group__KinematicRepresentationLib.html
new file mode 100644
index 000000000..af359ff7e
--- /dev/null
+++ b/group__KinematicRepresentationLib.html
@@ -0,0 +1,84 @@
+
+
+
+
+
+
+
+kinematics-dynamics: KinematicRepresentationLib
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Contains utilities related to conversion mechanisms between different coordinate and orientation systems.
+
+
+
+
+
+
diff --git a/group__ScrewTheoryLib.html b/group__ScrewTheoryLib.html
new file mode 100644
index 000000000..3cdd9af82
--- /dev/null
+++ b/group__ScrewTheoryLib.html
@@ -0,0 +1,230 @@
+
+
+
+
+
+
+
+kinematics-dynamics: ScrewTheoryLib
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Contains classes related to Screw Theory solvers and tools.
+More...
+
+
+
Proof of concept for a kinematics library based on screw theory concepts applied to robotics. This implementation is mainly focused on solving inverse kinematics problems in an efficient and effective manner via closed-form geometric solutions. Comparing this approach with a tradicional numeric-based approach yields ([4] ):
+faster solutions, since there are no iterations involved;
+exact solutions, since the direct formulation guarantees convergence;
+multiple solutions and the possibility to choose the better ones.
+
+
See also [1]
+
+
+
◆ normalizeAngle()
+
+
+
+
+
+
+
+
+ double roboticslab::normalizeAngle
+ (
+ double
+ angle )
+
+
+
+
+
+inline
+
+
+
+
Values that exceed [-pi pi] are normalized to fit this range. Values close to -pi are approximated as pi.
+
Parameters
+
+ angle Magnitude of the requested angle.
+
+
+
+
Returns Normalized angle.
+
+
+
+
+
◆ vectorPow2()
+
+
+
+
+
+
+
+
+ KDL::Rotation roboticslab::vectorPow2
+ (
+ const KDL::Vector &
+ v )
+
+
+
+
+
+inline
+
+
+
+
Given a column-vector v, computes v*v'.
+
Parameters
+
+
+
+
Returns Resulting square matrix.
+
+
+
+
+
+
+
+
diff --git a/group__TinyMath.html b/group__TinyMath.html
new file mode 100644
index 000000000..188294cde
--- /dev/null
+++ b/group__TinyMath.html
@@ -0,0 +1,84 @@
+
+
+
+
+
+
+
+kinematics-dynamics: TinyMath
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
A small mathematical library, mostly for performing transformations between angle representations.
+
+
+
+
+
+
diff --git a/group__TrajGen.html b/group__TrajGen.html
new file mode 100644
index 000000000..c86d9c585
--- /dev/null
+++ b/group__TrajGen.html
@@ -0,0 +1,101 @@
+
+
+
+
+
+
+
+kinematics-dynamics: TrajGen
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
The TrajGen library is a collection of classes for trajectory generation.
+More...
+
+
+
The TrajGen library is a collection of classes for trajectory generation. For now the functions' implementations are so short that they have been programmed inline, so for now the library is actually header-based (no actual real static/dynamic library to be linked).
+
+
+
+
+
diff --git a/group__YarpPlugins.html b/group__YarpPlugins.html
new file mode 100644
index 000000000..a37a104da
--- /dev/null
+++ b/group__YarpPlugins.html
@@ -0,0 +1,119 @@
+
+
+
+
+
+
+
+kinematics-dynamics: YarpPlugins
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Contains kinematics-dynamics libraries that implement YARP device interfaces and therefore can be invoked as YARP plugins.
+More...
+
+
+
+
+
+
+
diff --git a/group__cartesianControlExample.html b/group__cartesianControlExample.html
new file mode 100644
index 000000000..231780795
--- /dev/null
+++ b/group__cartesianControlExample.html
@@ -0,0 +1,86 @@
+
+
+
+
+
+
+
+kinematics-dynamics: cartesianControlExample
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Running example with teoSim First we must run a YARP name server if it is not running in our current namespace:
[on terminal 1] yarp server
+ What mostly changes is the library command line invocation. We also change the server port name. The following is an example for the simulated robot's right arm.
[on terminal 2] teoSim
+[on terminal 3] yarpdev --device BasicCartesianControl --name /teoSim/rightArm/CartesianControl --kinematics teo-fixedTrunk-rightArm-fetch.ini --local /BasicCartesianControl/teoSim/rightArm --remote /teoSim/rightArm
+[on terminal 4] ./cartesianControlExample
+[on possible terminal 5] yarp rpc /teoSim/rightArm/CartesianControl/rpc:s (for manual operation)
+
+
+
+
+
diff --git a/group__exampleYarpTinyMath.html b/group__exampleYarpTinyMath.html
new file mode 100644
index 000000000..790d5b029
--- /dev/null
+++ b/group__exampleYarpTinyMath.html
@@ -0,0 +1,97 @@
+
+
+
+
+
+
+
+kinematics-dynamics: exampleYarpTinyMath
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This is an example of the use of YarpTinyMath.
+
+
+Legal
+
Copyright: (C) 2013 Universidad Carlos III de Madrid
+
Author: Juan G. Victores
+
CopyPolicy: Released under the terms of the LGPLv2.1 or later, see license/LGPL.TXT
+
+Building
+
cd examples/cpp/exampleYarpTinyMath/
+mkdir build; cd build; cmake ..
+make -j3
+
+Running
+
./exampleYarpTinyMath
+
+
+
+
+
diff --git a/group__ftCompensation.html b/group__ftCompensation.html
new file mode 100644
index 000000000..4f6b09db1
--- /dev/null
+++ b/group__ftCompensation.html
@@ -0,0 +1,94 @@
+
+
+
+
+
+
+
+kinematics-dynamics: ftCompensation
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Creates an instance of roboticslab::FtCompensation .
+More...
+
+
+class roboticslab::FtCompensation
+ Produces motion in the direction of an externally applied force measured by a force-torque sensor (pretty much mimicking a classical gravity compensation app). More...
+
+
+
+
+
+
+
+
diff --git a/group__haarDetectionController.html b/group__haarDetectionController.html
new file mode 100644
index 000000000..b4bb0cafc
--- /dev/null
+++ b/group__haarDetectionController.html
@@ -0,0 +1,97 @@
+
+
+
+
+
+
+
+kinematics-dynamics: haarDetectionController
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/group__keyboardController.html b/group__keyboardController.html
new file mode 100644
index 000000000..8812133c4
--- /dev/null
+++ b/group__keyboardController.html
@@ -0,0 +1,97 @@
+
+
+
+
+
+
+
+kinematics-dynamics: keyboardController
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/group__kinematics-dynamics-applications.html b/group__kinematics-dynamics-applications.html
new file mode 100644
index 000000000..3da975725
--- /dev/null
+++ b/group__kinematics-dynamics-applications.html
@@ -0,0 +1,84 @@
+
+
+
+
+
+
+
+kinematics-dynamics: kinematics-dynamics Applications (Collections of Programs)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
kinematics-dynamics applications (collections of programs).
+
+
+
+
+
+
diff --git a/group__kinematics-dynamics-examples.html b/group__kinematics-dynamics-examples.html
new file mode 100644
index 000000000..5ce177782
--- /dev/null
+++ b/group__kinematics-dynamics-examples.html
@@ -0,0 +1,95 @@
+
+
+
+
+
+
+
+kinematics-dynamics: kinematics-dynamics Examples
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
kinematics-dynamics examples.
+More...
+
+
+
+
+
+
+
diff --git a/group__kinematics-dynamics-libraries.html b/group__kinematics-dynamics-libraries.html
new file mode 100644
index 000000000..7c930832a
--- /dev/null
+++ b/group__kinematics-dynamics-libraries.html
@@ -0,0 +1,109 @@
+
+
+
+
+
+
+
+kinematics-dynamics: kinematics-dynamics Libraries
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
kinematics-dynamics libraries.
+More...
+
+
+ YarpPlugins
+ Contains kinematics-dynamics libraries that implement YARP device interfaces and therefore can be invoked as YARP plugins.
+
+ KdlVectorConverterLib
+ Contains classes related to KDL and std::vector classes.
+
+ KinematicRepresentationLib
+ Contains utilities related to conversion mechanisms between different coordinate and orientation systems.
+
+ ScrewTheoryLib
+ Contains classes related to Screw Theory solvers and tools.
+
+ TrajGen
+ The TrajGen library is a collection of classes for trajectory generation.
+
+ TinyMath
+ A small mathematical library, mostly for performing transformations between angle representations.
+
+
+
+
+
+
+
+
diff --git a/group__kinematics-dynamics-programs.html b/group__kinematics-dynamics-programs.html
new file mode 100644
index 000000000..d33a596e6
--- /dev/null
+++ b/group__kinematics-dynamics-programs.html
@@ -0,0 +1,103 @@
+
+
+
+
+
+
+
+kinematics-dynamics: kinematics-dynamics Programs
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
kinematics-dynamics programs.
+More...
+
+
+
+
+
+
+
diff --git a/group__kinematics-dynamics-tests.html b/group__kinematics-dynamics-tests.html
new file mode 100644
index 000000000..41b7ed5ac
--- /dev/null
+++ b/group__kinematics-dynamics-tests.html
@@ -0,0 +1,109 @@
+
+
+
+
+
+
+
+kinematics-dynamics: kinematics-dynamics Tests
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
kinematics-dynamics tests.
+More...
+
+
+
+
+
+
+
diff --git a/group__screwTheoryTrajectoryExample.html b/group__screwTheoryTrajectoryExample.html
new file mode 100644
index 000000000..1db99ff02
--- /dev/null
+++ b/group__screwTheoryTrajectoryExample.html
@@ -0,0 +1,91 @@
+
+
+
+
+
+
+
+kinematics-dynamics: screwTheoryTrajectoryExample
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Legal
+
Copyright: (C) 2018 Universidad Carlos III de Madrid;
+
CopyPolicy: Released under the terms of the LGPLv2.1 or later, see license/LGPL.TXT
+
Building
cd examples/cpp/exampleScrewTheoryTrajectory/
+mkdir build; cd build; cmake ..
+make -j$(nproc)
+ Running example with teoSim First we must run a YARP name server if it is not running in our current namespace:
[on terminal 1] yarp server
+ What mostly changes is the library command line invocation. We also change the server port name. The following is an example for the simulated robot's right arm.
[on terminal 2] teoSim
+[on terminal 3] yarpdev --device BasicCartesianControl --name /teoSim/leftArm/CartesianControl --kinematics teo-fixedTrunk-leftArm-fetch.ini --robot remote_controlboard --local /BasicCartesianControl/teoSim/leftArm --remote /teoSim/leftArm --angleRepr axisAngle
+[on terminal 4] ./exampleScrewTheoryTrajectory
+
+
+
+
+
diff --git a/group__streamingDeviceController.html b/group__streamingDeviceController.html
new file mode 100644
index 000000000..dd35404ed
--- /dev/null
+++ b/group__streamingDeviceController.html
@@ -0,0 +1,114 @@
+
+
+
+
+
+
+
+kinematics-dynamics: streamingDeviceController
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/hierarchy.html b/hierarchy.html
new file mode 100644
index 000000000..24c08906e
--- /dev/null
+++ b/hierarchy.html
@@ -0,0 +1,169 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Class Hierarchy
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
This inheritance list is sorted roughly, but not completely, alphabetically:
+
+
+
+
+
diff --git a/index.html b/index.html
new file mode 100644
index 000000000..0edf55898
--- /dev/null
+++ b/index.html
@@ -0,0 +1,235 @@
+
+
+
+
+
+
+
+kinematics-dynamics: Main Page
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Kinematics and dynamics solvers and controllers.
+
Link to Doxygen generated documentation: https://robots.uc3m.es/kinematics-dynamics/
+
+
+
+Installation
+
Installation instructions for installing from source can be found here .
+
+Contributing
+
+Posting Issues
+
+Read CONTRIBUTING.md
+Post an issue / Feature request / Specific documentation request
+
+
+Fork & Pull Request
+
+Fork the repository
+Create your feature branch (git checkout -b my-new-feature
) off the master
branch, following the Forking Git workflow
+Commit your changes
+Push to the branch (git push origin my-new-feature
)
+Create a new Pull Request
+
+
+Citation
+
If you found this project useful, please consider citing the following works:
+
+
Bartek Lukawski, Ignacio Montesino Valle, Juan G. Victores, Alberto Jardón, and Carlos Balaguer. An inverse kinematics problem solver based on screw theory for manipulator arms. In XLIII Jornadas de Automática , pages 864–869. Universidade da Coruña, 2022.
+
@inproceedings{lukawski2022jjaa,
+
author = {{\L}ukawski, Bartek and Montesino Valle, Ignacio and Victores, Juan G. and Jardón, Alberto and Balaguer, Carlos},
+
title = {An inverse kinematics problem solver based on screw theory for manipulator arms},
+
booktitle = {XLIII Jornadas de Automática},
+
year = {2022},
+
pages = {864--869},
+
publisher = {Universidade da Coruña},
+
doi = {10.17979/spudc.9788497498418.0864},
+
}
+
+
Edwin Daniel Oña, Bartek Lukawski, Alberto Jardón, and Carlos Balaguer. A modular framework to facilitate the control of an assistive robotic arm using visual servoing and proximity sensing. In IEEE Int. Conf. on Autonomous Robot Systems and Competitions (ICARSC) , pages 28–33, 2020.
+
@inproceedings{eona2020icarsc,
+
author = {{O\~na}, Edwin Daniel and {\L}ukawski, Bartek and Jardón, Alberto and Balaguer, Carlos},
+
title = {A modular framework to facilitate the control of an assistive robotic arm using visual servoing and proximity sensing},
+
booktitle = {IEEE Int. Conf. on Autonomous Robot Systems and Competitions (ICARSC)},
+
year = {2020},
+
pages = {28--33},
+
doi = {10.1109/ICARSC49921.2020.9096146},
+
}
+
Bartek Lukawski, Juan G. Victores, and Carlos Balaguer. A generic controller for teleoperation on robotic manipulators using low-cost devices. In XLIV Jornadas de Automática , pages 785–788. Universidade da Coruña, 2023.
+
@inproceedings{lukawski2023jjaa,
+
author = {{\L}ukawski, Bartek and Victores, Juan G. and Balaguer, Carlos},
+
title = {A generic controller for teleoperation on robotic manipulators using low-cost devices},
+
booktitle = {XLIV Jornadas de Automática},
+
year = {2023},
+
pages = {785--788},
+
publisher = {Universidade da Coruña},
+
doi = {10.17979/spudc.9788497498609.785},
+
}
+
+Status
+
+
+
+
+Similar and Related Projects
+
+Quaternions
+
+
+Fast Solvers
+
+
+IK-Solvers
+
+
+Kinematics and Dynamics
+
+
+Path-Planning, Trajectory generation and optimization
+
+
+Humanoid-oriented
+
+
+
+
+
+
+
diff --git a/jquery.js b/jquery.js
new file mode 100644
index 000000000..103c32d79
--- /dev/null
+++ b/jquery.js
@@ -0,0 +1,35 @@
+/*! jQuery v3.4.1 | (c) JS Foundation and other contributors | jquery.org/license */
+!function(e,t){"use strict";"object"==typeof module&&"object"==typeof module.exports?module.exports=e.document?t(e,!0):function(e){if(!e.document)throw new Error("jQuery requires a window with a document");return t(e)}:t(e)}("undefined"!=typeof window?window:this,function(C,e){"use strict";var t=[],E=C.document,r=Object.getPrototypeOf,s=t.slice,g=t.concat,u=t.push,i=t.indexOf,n={},o=n.toString,v=n.hasOwnProperty,a=v.toString,l=a.call(Object),y={},m=function(e){return"function"==typeof e&&"number"!=typeof e.nodeType},x=function(e){return null!=e&&e===e.window},c={type:!0,src:!0,nonce:!0,noModule:!0};function b(e,t,n){var r,i,o=(n=n||E).createElement("script");if(o.text=e,t)for(r in c)(i=t[r]||t.getAttribute&&t.getAttribute(r))&&o.setAttribute(r,i);n.head.appendChild(o).parentNode.removeChild(o)}function w(e){return null==e?e+"":"object"==typeof e||"function"==typeof e?n[o.call(e)]||"object":typeof e}var f="3.4.1",k=function(e,t){return new k.fn.init(e,t)},p=/^[\s\uFEFF\xA0]+|[\s\uFEFF\xA0]+$/g;function d(e){var t=!!e&&"length"in e&&e.length,n=w(e);return!m(e)&&!x(e)&&("array"===n||0===t||"number"==typeof t&&0+~]|"+M+")"+M+"*"),U=new RegExp(M+"|>"),X=new RegExp($),V=new RegExp("^"+I+"$"),G={ID:new RegExp("^#("+I+")"),CLASS:new RegExp("^\\.("+I+")"),TAG:new RegExp("^("+I+"|[*])"),ATTR:new RegExp("^"+W),PSEUDO:new RegExp("^"+$),CHILD:new RegExp("^:(only|first|last|nth|nth-last)-(child|of-type)(?:\\("+M+"*(even|odd|(([+-]|)(\\d*)n|)"+M+"*(?:([+-]|)"+M+"*(\\d+)|))"+M+"*\\)|)","i"),bool:new RegExp("^(?:"+R+")$","i"),needsContext:new RegExp("^"+M+"*[>+~]|:(even|odd|eq|gt|lt|nth|first|last)(?:\\("+M+"*((?:-\\d)?\\d*)"+M+"*\\)|)(?=[^-]|$)","i")},Y=/HTML$/i,Q=/^(?:input|select|textarea|button)$/i,J=/^h\d$/i,K=/^[^{]+\{\s*\[native \w/,Z=/^(?:#([\w-]+)|(\w+)|\.([\w-]+))$/,ee=/[+~]/,te=new RegExp("\\\\([\\da-f]{1,6}"+M+"?|("+M+")|.)","ig"),ne=function(e,t,n){var r="0x"+t-65536;return r!=r||n?t:r<0?String.fromCharCode(r+65536):String.fromCharCode(r>>10|55296,1023&r|56320)},re=/([\0-\x1f\x7f]|^-?\d)|^-$|[^\0-\x1f\x7f-\uFFFF\w-]/g,ie=function(e,t){return t?"\0"===e?"\ufffd":e.slice(0,-1)+"\\"+e.charCodeAt(e.length-1).toString(16)+" ":"\\"+e},oe=function(){T()},ae=be(function(e){return!0===e.disabled&&"fieldset"===e.nodeName.toLowerCase()},{dir:"parentNode",next:"legend"});try{H.apply(t=O.call(m.childNodes),m.childNodes),t[m.childNodes.length].nodeType}catch(e){H={apply:t.length?function(e,t){L.apply(e,O.call(t))}:function(e,t){var n=e.length,r=0;while(e[n++]=t[r++]);e.length=n-1}}}function se(t,e,n,r){var i,o,a,s,u,l,c,f=e&&e.ownerDocument,p=e?e.nodeType:9;if(n=n||[],"string"!=typeof t||!t||1!==p&&9!==p&&11!==p)return n;if(!r&&((e?e.ownerDocument||e:m)!==C&&T(e),e=e||C,E)){if(11!==p&&(u=Z.exec(t)))if(i=u[1]){if(9===p){if(!(a=e.getElementById(i)))return n;if(a.id===i)return n.push(a),n}else if(f&&(a=f.getElementById(i))&&y(e,a)&&a.id===i)return n.push(a),n}else{if(u[2])return H.apply(n,e.getElementsByTagName(t)),n;if((i=u[3])&&d.getElementsByClassName&&e.getElementsByClassName)return H.apply(n,e.getElementsByClassName(i)),n}if(d.qsa&&!A[t+" "]&&(!v||!v.test(t))&&(1!==p||"object"!==e.nodeName.toLowerCase())){if(c=t,f=e,1===p&&U.test(t)){(s=e.getAttribute("id"))?s=s.replace(re,ie):e.setAttribute("id",s=k),o=(l=h(t)).length;while(o--)l[o]="#"+s+" "+xe(l[o]);c=l.join(","),f=ee.test(t)&&ye(e.parentNode)||e}try{return H.apply(n,f.querySelectorAll(c)),n}catch(e){A(t,!0)}finally{s===k&&e.removeAttribute("id")}}}return g(t.replace(B,"$1"),e,n,r)}function ue(){var r=[];return function e(t,n){return r.push(t+" ")>b.cacheLength&&delete e[r.shift()],e[t+" "]=n}}function le(e){return e[k]=!0,e}function ce(e){var t=C.createElement("fieldset");try{return!!e(t)}catch(e){return!1}finally{t.parentNode&&t.parentNode.removeChild(t),t=null}}function fe(e,t){var n=e.split("|"),r=n.length;while(r--)b.attrHandle[n[r]]=t}function pe(e,t){var n=t&&e,r=n&&1===e.nodeType&&1===t.nodeType&&e.sourceIndex-t.sourceIndex;if(r)return r;if(n)while(n=n.nextSibling)if(n===t)return-1;return e?1:-1}function de(t){return function(e){return"input"===e.nodeName.toLowerCase()&&e.type===t}}function he(n){return function(e){var t=e.nodeName.toLowerCase();return("input"===t||"button"===t)&&e.type===n}}function ge(t){return function(e){return"form"in e?e.parentNode&&!1===e.disabled?"label"in e?"label"in e.parentNode?e.parentNode.disabled===t:e.disabled===t:e.isDisabled===t||e.isDisabled!==!t&&ae(e)===t:e.disabled===t:"label"in e&&e.disabled===t}}function ve(a){return le(function(o){return o=+o,le(function(e,t){var n,r=a([],e.length,o),i=r.length;while(i--)e[n=r[i]]&&(e[n]=!(t[n]=e[n]))})})}function ye(e){return e&&"undefined"!=typeof e.getElementsByTagName&&e}for(e in d=se.support={},i=se.isXML=function(e){var t=e.namespaceURI,n=(e.ownerDocument||e).documentElement;return!Y.test(t||n&&n.nodeName||"HTML")},T=se.setDocument=function(e){var t,n,r=e?e.ownerDocument||e:m;return r!==C&&9===r.nodeType&&r.documentElement&&(a=(C=r).documentElement,E=!i(C),m!==C&&(n=C.defaultView)&&n.top!==n&&(n.addEventListener?n.addEventListener("unload",oe,!1):n.attachEvent&&n.attachEvent("onunload",oe)),d.attributes=ce(function(e){return e.className="i",!e.getAttribute("className")}),d.getElementsByTagName=ce(function(e){return e.appendChild(C.createComment("")),!e.getElementsByTagName("*").length}),d.getElementsByClassName=K.test(C.getElementsByClassName),d.getById=ce(function(e){return a.appendChild(e).id=k,!C.getElementsByName||!C.getElementsByName(k).length}),d.getById?(b.filter.ID=function(e){var t=e.replace(te,ne);return function(e){return e.getAttribute("id")===t}},b.find.ID=function(e,t){if("undefined"!=typeof t.getElementById&&E){var n=t.getElementById(e);return n?[n]:[]}}):(b.filter.ID=function(e){var n=e.replace(te,ne);return function(e){var t="undefined"!=typeof e.getAttributeNode&&e.getAttributeNode("id");return t&&t.value===n}},b.find.ID=function(e,t){if("undefined"!=typeof t.getElementById&&E){var n,r,i,o=t.getElementById(e);if(o){if((n=o.getAttributeNode("id"))&&n.value===e)return[o];i=t.getElementsByName(e),r=0;while(o=i[r++])if((n=o.getAttributeNode("id"))&&n.value===e)return[o]}return[]}}),b.find.TAG=d.getElementsByTagName?function(e,t){return"undefined"!=typeof t.getElementsByTagName?t.getElementsByTagName(e):d.qsa?t.querySelectorAll(e):void 0}:function(e,t){var n,r=[],i=0,o=t.getElementsByTagName(e);if("*"===e){while(n=o[i++])1===n.nodeType&&r.push(n);return r}return o},b.find.CLASS=d.getElementsByClassName&&function(e,t){if("undefined"!=typeof t.getElementsByClassName&&E)return t.getElementsByClassName(e)},s=[],v=[],(d.qsa=K.test(C.querySelectorAll))&&(ce(function(e){a.appendChild(e).innerHTML=" ",e.querySelectorAll("[msallowcapture^='']").length&&v.push("[*^$]="+M+"*(?:''|\"\")"),e.querySelectorAll("[selected]").length||v.push("\\["+M+"*(?:value|"+R+")"),e.querySelectorAll("[id~="+k+"-]").length||v.push("~="),e.querySelectorAll(":checked").length||v.push(":checked"),e.querySelectorAll("a#"+k+"+*").length||v.push(".#.+[+~]")}),ce(function(e){e.innerHTML=" ";var t=C.createElement("input");t.setAttribute("type","hidden"),e.appendChild(t).setAttribute("name","D"),e.querySelectorAll("[name=d]").length&&v.push("name"+M+"*[*^$|!~]?="),2!==e.querySelectorAll(":enabled").length&&v.push(":enabled",":disabled"),a.appendChild(e).disabled=!0,2!==e.querySelectorAll(":disabled").length&&v.push(":enabled",":disabled"),e.querySelectorAll("*,:x"),v.push(",.*:")})),(d.matchesSelector=K.test(c=a.matches||a.webkitMatchesSelector||a.mozMatchesSelector||a.oMatchesSelector||a.msMatchesSelector))&&ce(function(e){d.disconnectedMatch=c.call(e,"*"),c.call(e,"[s!='']:x"),s.push("!=",$)}),v=v.length&&new RegExp(v.join("|")),s=s.length&&new RegExp(s.join("|")),t=K.test(a.compareDocumentPosition),y=t||K.test(a.contains)?function(e,t){var n=9===e.nodeType?e.documentElement:e,r=t&&t.parentNode;return e===r||!(!r||1!==r.nodeType||!(n.contains?n.contains(r):e.compareDocumentPosition&&16&e.compareDocumentPosition(r)))}:function(e,t){if(t)while(t=t.parentNode)if(t===e)return!0;return!1},D=t?function(e,t){if(e===t)return l=!0,0;var n=!e.compareDocumentPosition-!t.compareDocumentPosition;return n||(1&(n=(e.ownerDocument||e)===(t.ownerDocument||t)?e.compareDocumentPosition(t):1)||!d.sortDetached&&t.compareDocumentPosition(e)===n?e===C||e.ownerDocument===m&&y(m,e)?-1:t===C||t.ownerDocument===m&&y(m,t)?1:u?P(u,e)-P(u,t):0:4&n?-1:1)}:function(e,t){if(e===t)return l=!0,0;var n,r=0,i=e.parentNode,o=t.parentNode,a=[e],s=[t];if(!i||!o)return e===C?-1:t===C?1:i?-1:o?1:u?P(u,e)-P(u,t):0;if(i===o)return pe(e,t);n=e;while(n=n.parentNode)a.unshift(n);n=t;while(n=n.parentNode)s.unshift(n);while(a[r]===s[r])r++;return r?pe(a[r],s[r]):a[r]===m?-1:s[r]===m?1:0}),C},se.matches=function(e,t){return se(e,null,null,t)},se.matchesSelector=function(e,t){if((e.ownerDocument||e)!==C&&T(e),d.matchesSelector&&E&&!A[t+" "]&&(!s||!s.test(t))&&(!v||!v.test(t)))try{var n=c.call(e,t);if(n||d.disconnectedMatch||e.document&&11!==e.document.nodeType)return n}catch(e){A(t,!0)}return 0":{dir:"parentNode",first:!0}," ":{dir:"parentNode"},"+":{dir:"previousSibling",first:!0},"~":{dir:"previousSibling"}},preFilter:{ATTR:function(e){return e[1]=e[1].replace(te,ne),e[3]=(e[3]||e[4]||e[5]||"").replace(te,ne),"~="===e[2]&&(e[3]=" "+e[3]+" "),e.slice(0,4)},CHILD:function(e){return e[1]=e[1].toLowerCase(),"nth"===e[1].slice(0,3)?(e[3]||se.error(e[0]),e[4]=+(e[4]?e[5]+(e[6]||1):2*("even"===e[3]||"odd"===e[3])),e[5]=+(e[7]+e[8]||"odd"===e[3])):e[3]&&se.error(e[0]),e},PSEUDO:function(e){var t,n=!e[6]&&e[2];return G.CHILD.test(e[0])?null:(e[3]?e[2]=e[4]||e[5]||"":n&&X.test(n)&&(t=h(n,!0))&&(t=n.indexOf(")",n.length-t)-n.length)&&(e[0]=e[0].slice(0,t),e[2]=n.slice(0,t)),e.slice(0,3))}},filter:{TAG:function(e){var t=e.replace(te,ne).toLowerCase();return"*"===e?function(){return!0}:function(e){return e.nodeName&&e.nodeName.toLowerCase()===t}},CLASS:function(e){var t=p[e+" "];return t||(t=new RegExp("(^|"+M+")"+e+"("+M+"|$)"))&&p(e,function(e){return t.test("string"==typeof e.className&&e.className||"undefined"!=typeof e.getAttribute&&e.getAttribute("class")||"")})},ATTR:function(n,r,i){return function(e){var t=se.attr(e,n);return null==t?"!="===r:!r||(t+="","="===r?t===i:"!="===r?t!==i:"^="===r?i&&0===t.indexOf(i):"*="===r?i&&-1:\x20\t\r\n\f]*)[\x20\t\r\n\f]*\/?>(?:<\/\1>|)$/i;function j(e,n,r){return m(n)?k.grep(e,function(e,t){return!!n.call(e,t,e)!==r}):n.nodeType?k.grep(e,function(e){return e===n!==r}):"string"!=typeof n?k.grep(e,function(e){return-1)[^>]*|#([\w-]+))$/;(k.fn.init=function(e,t,n){var r,i;if(!e)return this;if(n=n||q,"string"==typeof e){if(!(r="<"===e[0]&&">"===e[e.length-1]&&3<=e.length?[null,e,null]:L.exec(e))||!r[1]&&t)return!t||t.jquery?(t||n).find(e):this.constructor(t).find(e);if(r[1]){if(t=t instanceof k?t[0]:t,k.merge(this,k.parseHTML(r[1],t&&t.nodeType?t.ownerDocument||t:E,!0)),D.test(r[1])&&k.isPlainObject(t))for(r in t)m(this[r])?this[r](t[r]):this.attr(r,t[r]);return this}return(i=E.getElementById(r[2]))&&(this[0]=i,this.length=1),this}return e.nodeType?(this[0]=e,this.length=1,this):m(e)?void 0!==n.ready?n.ready(e):e(k):k.makeArray(e,this)}).prototype=k.fn,q=k(E);var H=/^(?:parents|prev(?:Until|All))/,O={children:!0,contents:!0,next:!0,prev:!0};function P(e,t){while((e=e[t])&&1!==e.nodeType);return e}k.fn.extend({has:function(e){var t=k(e,this),n=t.length;return this.filter(function(){for(var e=0;e\x20\t\r\n\f]*)/i,he=/^$|^module$|\/(?:java|ecma)script/i,ge={option:[1,""," "],thead:[1,""],col:[2,""],tr:[2,""],td:[3,""],_default:[0,"",""]};function ve(e,t){var n;return n="undefined"!=typeof e.getElementsByTagName?e.getElementsByTagName(t||"*"):"undefined"!=typeof e.querySelectorAll?e.querySelectorAll(t||"*"):[],void 0===t||t&&A(e,t)?k.merge([e],n):n}function ye(e,t){for(var n=0,r=e.length;nx",y.noCloneChecked=!!me.cloneNode(!0).lastChild.defaultValue;var Te=/^key/,Ce=/^(?:mouse|pointer|contextmenu|drag|drop)|click/,Ee=/^([^.]*)(?:\.(.+)|)/;function ke(){return!0}function Se(){return!1}function Ne(e,t){return e===function(){try{return E.activeElement}catch(e){}}()==("focus"===t)}function Ae(e,t,n,r,i,o){var a,s;if("object"==typeof t){for(s in"string"!=typeof n&&(r=r||n,n=void 0),t)Ae(e,s,n,r,t[s],o);return e}if(null==r&&null==i?(i=n,r=n=void 0):null==i&&("string"==typeof n?(i=r,r=void 0):(i=r,r=n,n=void 0)),!1===i)i=Se;else if(!i)return e;return 1===o&&(a=i,(i=function(e){return k().off(e),a.apply(this,arguments)}).guid=a.guid||(a.guid=k.guid++)),e.each(function(){k.event.add(this,t,i,r,n)})}function De(e,i,o){o?(Q.set(e,i,!1),k.event.add(e,i,{namespace:!1,handler:function(e){var t,n,r=Q.get(this,i);if(1&e.isTrigger&&this[i]){if(r.length)(k.event.special[i]||{}).delegateType&&e.stopPropagation();else if(r=s.call(arguments),Q.set(this,i,r),t=o(this,i),this[i](),r!==(n=Q.get(this,i))||t?Q.set(this,i,!1):n={},r!==n)return e.stopImmediatePropagation(),e.preventDefault(),n.value}else r.length&&(Q.set(this,i,{value:k.event.trigger(k.extend(r[0],k.Event.prototype),r.slice(1),this)}),e.stopImmediatePropagation())}})):void 0===Q.get(e,i)&&k.event.add(e,i,ke)}k.event={global:{},add:function(t,e,n,r,i){var o,a,s,u,l,c,f,p,d,h,g,v=Q.get(t);if(v){n.handler&&(n=(o=n).handler,i=o.selector),i&&k.find.matchesSelector(ie,i),n.guid||(n.guid=k.guid++),(u=v.events)||(u=v.events={}),(a=v.handle)||(a=v.handle=function(e){return"undefined"!=typeof k&&k.event.triggered!==e.type?k.event.dispatch.apply(t,arguments):void 0}),l=(e=(e||"").match(R)||[""]).length;while(l--)d=g=(s=Ee.exec(e[l])||[])[1],h=(s[2]||"").split(".").sort(),d&&(f=k.event.special[d]||{},d=(i?f.delegateType:f.bindType)||d,f=k.event.special[d]||{},c=k.extend({type:d,origType:g,data:r,handler:n,guid:n.guid,selector:i,needsContext:i&&k.expr.match.needsContext.test(i),namespace:h.join(".")},o),(p=u[d])||((p=u[d]=[]).delegateCount=0,f.setup&&!1!==f.setup.call(t,r,h,a)||t.addEventListener&&t.addEventListener(d,a)),f.add&&(f.add.call(t,c),c.handler.guid||(c.handler.guid=n.guid)),i?p.splice(p.delegateCount++,0,c):p.push(c),k.event.global[d]=!0)}},remove:function(e,t,n,r,i){var o,a,s,u,l,c,f,p,d,h,g,v=Q.hasData(e)&&Q.get(e);if(v&&(u=v.events)){l=(t=(t||"").match(R)||[""]).length;while(l--)if(d=g=(s=Ee.exec(t[l])||[])[1],h=(s[2]||"").split(".").sort(),d){f=k.event.special[d]||{},p=u[d=(r?f.delegateType:f.bindType)||d]||[],s=s[2]&&new RegExp("(^|\\.)"+h.join("\\.(?:.*\\.|)")+"(\\.|$)"),a=o=p.length;while(o--)c=p[o],!i&&g!==c.origType||n&&n.guid!==c.guid||s&&!s.test(c.namespace)||r&&r!==c.selector&&("**"!==r||!c.selector)||(p.splice(o,1),c.selector&&p.delegateCount--,f.remove&&f.remove.call(e,c));a&&!p.length&&(f.teardown&&!1!==f.teardown.call(e,h,v.handle)||k.removeEvent(e,d,v.handle),delete u[d])}else for(d in u)k.event.remove(e,d+t[l],n,r,!0);k.isEmptyObject(u)&&Q.remove(e,"handle events")}},dispatch:function(e){var t,n,r,i,o,a,s=k.event.fix(e),u=new Array(arguments.length),l=(Q.get(this,"events")||{})[s.type]||[],c=k.event.special[s.type]||{};for(u[0]=s,t=1;t\x20\t\r\n\f]*)[^>]*)\/>/gi,qe=/
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ kinematics-dynamics
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
1 #ifndef __SCREW_THEORY_LOG_COMPONENT_HPP__
+
2 #define __SCREW_THEORY_LOG_COMPONENT_HPP__
+
+
4 #include <yarp/os/LogComponent.h>
+
+
6 YARP_DECLARE_LOG_COMPONENT(ST)
+
+
+
+
+
+
+