12 static const double L0 = 1.0;
13 static const double L1 = 0.5;
14 static const double L2 = 0.4;
15 static const double L3 = 0;
16 static const double L4 = 0;
17 static const double L5 = 0;
52 Vector(0.0,-0.3120511,-0.0038871),
59 Vector(0.0,-0.0015515,0.0),
95 switch(representation)
116 for(
unsigned int l=0;l<6;l++)
117 for(
unsigned int c=0;c<J_q.
columns();c++)
118 Jdot(l,c) = (J_qdt(l,c) - J_q(l,c))/dt;
126 Jdot(0,0) = -
L1 * (qdot(0) + qdot(1))*
cos(q(0)+q(1))-
L0*
cos(q(0))*qdot(0);
127 Jdot(0,1) = -
L1 * (qdot(0) + qdot(1))*
cos(q(0)+q(1));
128 Jdot(1,0) = -
L1 * (qdot(0) + qdot(1))*
sin(q(0)+q(1))-
L0*
sin(q(0))*qdot(0);
129 Jdot(1,1) = -
L1 * (qdot(0) + qdot(1))*
sin(q(0)+q(1));
139 J(0,1) = -
L1 *
sin(q(0)+q(1));
141 J(1,1) =
L1 *
cos(q(0)+q(1));
149 for(
unsigned int i=0; i<q.
rows(); i++)
150 q_qdqt(i) += dt*qdot(i);
156 for(
unsigned int i=0; i<q.
rows(); i++)
170 q_dqdt =
diff(q,qdot,dt);
176 Frame F_bs_ee_q,F_bs_ee_q_dqdt;
182 j_solver.
JntToJac(q_dqdt,jac_q_dqdt);
185 fk_solver.
JntToCart(q_dqdt,F_bs_ee_q_dqdt);
190 Jdot_diff(jac_q,jac_q_dqdt,dt,jdot_by_diff);
196 Twist jdot_qdot_by_solver;
199 Twist jdot_qdot_by_diff;
203 std::cout <<
"Jdot diff:\n" << jdot_by_diff <<
"\n" 204 <<
"Jdot solver:\n" << jdot_by_solver <<
"\n" 206 << jdot_qdot_by_diff-jdot_qdot_by_solver <<
"\n" 210 double err = jdot_qdot_by_diff.
vel.
Norm() - jdot_qdot_by_solver.
vel.
Norm()
231 Twist jdot_qdot_by_solver;
238 std::cout <<
"Jdot symbolic:\n" << jdot_sym <<
"\n" 239 <<
"Jdot solver:\n" << jdot_by_solver <<
"\n" 240 <<
"Error:\n" << jdot_qdot_sym-jdot_qdot_by_solver <<
"\n" 242 << qdot << std::endl;
244 double err = jdot_qdot_sym.
vel.
Norm() - jdot_qdot_by_solver.
vel.
Norm()
252 bool verbose =
false;
254 bool print_err =
false;
256 for(
double dt=1e-6;dt<0.1;dt*=10)
258 double eps_diff_vs_solver = 4.0*dt;
260 for(
int i=0; i<100 ; ++i)
264 success &= err<=eps_diff_vs_solver;
266 if(!success || print_err){
267 std::cout <<
"dt: "<< dt <<
"\n" 268 <<
"err: "<< err <<
"\n" 269 <<
"eps_diff_vs_solver: " << eps_diff_vs_solver << std::endl;
311 bool verbose =
false;
313 bool print_err =
false;
315 double eps_sym_vs_solver = 1e-10;
317 for(
int i=0; i<100; ++i)
321 success &= err_d2_sym <= eps_sym_vs_solver;
323 if(!success || print_err){
324 std::cout <<
"err_d2_sym: " << err_d2_sym <<
"\n" 325 <<
"eps_sym_vs_solver: "<< eps_sym_vs_solver <<std::endl;
332 CPPUNIT_ASSERT(success);
void setRepresentation(const int &representation)
Sets the internal variable for the representation (with a check on the value)
void testD6DiffInertial()
static const int BODYFIXED
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body...
unsigned int rows() const
Computes the Jacobian time derivative (Jdot) by calculating the partial derivatives regarding to a jo...
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Vector vel
The velocity of that point.
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void MultiplyJacobian(const Jacobian &jac, const JntArray &src, Twist &dest)
void addSegment(const Segment &segment)
void testD2DiffInertial()
This class represents an fixed size array containing joint values of a KDL::Chain.
void testKukaDiffHybrid()
Jacobian Jdot_d2_symbolic(const JntArray &q, const JntArray &qdot)
void testD2DiffBodyFixed()
6D Inertia of a rigid body
double compare_d2_Jdot_Symbolic_vs_Solver(bool verbose)
void testKukaDiffBodyFixed()
CPPUNIT_TEST_SUITE_REGISTRATION(JacobianDotTest)
void Jdot_diff(const Jacobian &J_q, const Jacobian &J_qdt, const double &dt, Jacobian &Jdot)
unsigned int columns() const
Rotation M
Orientation of the Frame.
double Norm(double eps=epsilon) const
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
double compare_Jdot_Diff_vs_Solver(const Chain &chain, const double &dt, const int &representation, bool verbose)
represents both translational and rotational velocities.
Jacobian J_d2_symbolic(const JntArray &q, const JntArray &)
void testD6DiffBodyFixed()
IMETHOD void SetToZero(Vector &v)
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
A concrete implementation of a 3 dimensional vector class.
Vector rot
The rotational velocity of that point.
Vector p
origine of the Frame
friend bool changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
friend bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
unsigned int getNrOfJoints() const
static const int INERTIAL
bool runTest(const Chain &chain, const int &representation)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
virtual int JntToJacDot(const KDL::JntArrayVel &q_in, KDL::Twist &jac_dot_q_dot, int seg_nr=-1)
Computes .
void testKukaDiffInertial()
represents a frame transformation in 3D space (rotation + translation)
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
void changeRepresentation(Jacobian &J, const Frame &F_bs_ee, const int &representation)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)