15 #include <kdl/rall1d.h> 16 #include <kdl/rall1d_io.h> 17 #include <kdl/rall2d.h> 18 #include <kdl/rall2d_io.h> 19 #include <kdl/fvector.h> 20 #include <kdl/fvector_io.h> 21 #include <kdl/fvector2.h> 22 #include <kdl/fvector2_io.h> 23 #include <kdl/test_macros.h> 32 template <
class T,
int N>
34 public Rall1d<Rall1d<T, FVector<T,N> >, FVector2<Rall1d<T,FVector<T,N> >,N,T>,T>
42 this->t.grad= FVector<T,N>(d);
43 for (
int i=0;i<N;i++) {
44 this->grad[i].t = d[i];
45 this->grad[i].grad = FVector<T,N>(dd[i]);
53 public Rall1d< RallNd<N-1>, RallNd<N-1>, double >
60 this->t =
RallNd<N-1>(value,d);
61 this->grad =
RallNd<N-1>(d[0],&d[1]);
70 Rall1d<double,double,double>(arg) {}
89 KDL_DIFF( (a*a)*a, a*(a*a) );
90 KDL_DIFF( (-a)+a,T::Zero() );
91 KDL_DIFF( 2.0*a, a+a );
92 KDL_DIFF( a*2.0, a+a );
93 KDL_DIFF( a+2.0*a, 3.0*a );
94 KDL_DIFF( (a+2.0)*a, a*a +2.0*a );
95 KDL_DIFF( ((a+a)+a),(a+(a+a)) );
100 KDL_DIFF(
exp(
log(a)), a );
104 KDL_DIFF( 2.0*
sin(a)*
cos(a),
sin(2.0*a) );
105 KDL_DIFF( (a*a)/a, a );
106 KDL_DIFF( (a*a*a)/(a*a), a );
109 KDL_DIFF(
pow(
pow(a,3.0),1.0/3) , a );
110 KDL_DIFF(
hypot(3.0*a,4.0*a), 5.0*a);
121 const double pb[4] = { 1.0, 2.0, 3.0, 4.0 };
133 Rall2d<Rall1d<double>,Rall1d<double>,
double> g(Rall1d<double>(1,2),Rall1d<double>(2,3),Rall1d<double>(3,4));
138 Rall1d<
double, FVector<double,2> >(
140 FVector<double,2>(2,3)
142 FVector2<
Rall1d<
double,FVector<double,2> >,2,
double> (
143 Rall1d<
double,FVector<double,2> >(
145 FVector<double,2>(5,6)
147 Rall1d<
double,FVector<double,2> >(
149 FVector<double,2>(6,9)
156 double pj[] = {2.0,3.0,4.0};
157 double ppj[][3] = {{5.0,6.0,7.0},{6.0,8.0,9.0},{7.0,9.0,10.0}};
162 double pk[] = {2.0,3.0,4.0,5.0};
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > cosh(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > sinh(const Rall1d< T, V, S > &arg)
Rall2dN(T val, T d[N], T dd[N][N])
RallNd(const Rall1d< double > &arg)
INLINE Rall1d< T, V, S > tanh(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
RallNd(double value, double d[])
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
RallNd(const Rall1d< RallNd< N-1 >, RallNd< N-1 >, double > &arg)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
RallNd(double value, double d[])
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)