00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "jntarrayacc.hpp"
00023
00024 namespace KDL
00025 {
00026 JntArrayAcc::JntArrayAcc(unsigned int size):
00027 q(size),qdot(size),qdotdot(size)
00028 {
00029 }
00030 JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin,const JntArray& qdotdotin):
00031 q(qin),qdot(qdotin),qdotdot(qdotdotin)
00032 {
00033 assert(q.rows()==qdot.rows()&&qdot.rows()==qdotdot.rows());
00034 }
00035 JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin):
00036 q(qin),qdot(qdotin),qdotdot(q.rows())
00037 {
00038 assert(q.rows()==qdot.rows());
00039 }
00040 JntArrayAcc::JntArrayAcc(const JntArray& qin):
00041 q(qin),qdot(q.rows()),qdotdot(q.rows())
00042 {
00043 }
00044
00045 void JntArrayAcc::resize(unsigned int newSize)
00046 {
00047 q.resize(newSize);
00048 qdot.resize(newSize);
00049 qdotdot.resize(newSize);
00050 }
00051
00052 JntArray JntArrayAcc::value()const
00053 {
00054 return q;
00055 }
00056
00057 JntArray JntArrayAcc::deriv()const
00058 {
00059 return qdot;
00060 }
00061 JntArray JntArrayAcc::dderiv()const
00062 {
00063 return qdotdot;
00064 }
00065
00066 void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest)
00067 {
00068 Add(src1.q,src2.q,dest.q);
00069 Add(src1.qdot,src2.qdot,dest.qdot);
00070 Add(src1.qdotdot,src2.qdotdot,dest.qdotdot);
00071 }
00072 void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest)
00073 {
00074 Add(src1.q,src2.q,dest.q);
00075 Add(src1.qdot,src2.qdot,dest.qdot);
00076 dest.qdotdot=src1.qdotdot;
00077 }
00078 void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest)
00079 {
00080 Add(src1.q,src2,dest.q);
00081 dest.qdot=src1.qdot;
00082 dest.qdotdot=src1.qdotdot;
00083 }
00084
00085 void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest)
00086 {
00087 Subtract(src1.q,src2.q,dest.q);
00088 Subtract(src1.qdot,src2.qdot,dest.qdot);
00089 Subtract(src1.qdotdot,src2.qdotdot,dest.qdotdot);
00090 }
00091 void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest)
00092 {
00093 Subtract(src1.q,src2.q,dest.q);
00094 Subtract(src1.qdot,src2.qdot,dest.qdot);
00095 dest.qdotdot=src1.qdotdot;
00096 }
00097 void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest)
00098 {
00099 Subtract(src1.q,src2,dest.q);
00100 dest.qdot=src1.qdot;
00101 dest.qdotdot=src1.qdotdot;
00102 }
00103
00104 void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest)
00105 {
00106 Multiply(src.q,factor,dest.q);
00107 Multiply(src.qdot,factor,dest.qdot);
00108 Multiply(src.qdotdot,factor,dest.qdotdot);
00109 }
00110 void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest)
00111 {
00112 Multiply(src.qdot,factor.grad*2,dest.qdot);
00113 Multiply(src.qdotdot,factor.t,dest.qdotdot);
00114 Add(dest.qdot,dest.qdotdot,dest.qdotdot);
00115 Multiply(src.q,factor.grad,dest.q);
00116 Multiply(src.qdot,factor.t,dest.qdot);
00117 Add(dest.qdot,dest.q,dest.qdot);
00118 Multiply(src.q,factor.t,dest.q);
00119 }
00120 void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest)
00121 {
00122 Multiply(src.q,factor.dd,dest.q);
00123 Multiply(src.qdot,factor.d*2,dest.qdot);
00124 Multiply(src.qdotdot,factor.t,dest.qdotdot);
00125 Add(dest.qdotdot,dest.qdot,dest.qdotdot);
00126 Add(dest.qdotdot,dest.q,dest.qdotdot);
00127 Multiply(src.q,factor.d,dest.q);
00128 Multiply(src.qdot,factor.t,dest.qdot);
00129 Add(dest.qdot,dest.q,dest.qdot);
00130 Multiply(src.q,factor.t,dest.q);
00131 }
00132
00133 void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest)
00134 {
00135 Divide(src.q,factor,dest.q);
00136 Divide(src.qdot,factor,dest.qdot);
00137 Divide(src.qdotdot,factor,dest.qdotdot);
00138 }
00139 void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest)
00140 {
00141 Multiply(src.q,(2*factor.grad*factor.grad)/(factor.t*factor.t*factor.t),dest.q);
00142 Multiply(src.qdot,(2*factor.grad)/(factor.t*factor.t),dest.qdot);
00143 Divide(src.qdotdot,factor.t,dest.qdotdot);
00144 Subtract(dest.qdotdot,dest.qdot,dest.qdotdot);
00145 Add(dest.qdotdot,dest.q,dest.qdotdot);
00146 Multiply(src.q,factor.grad/(factor.t*factor.t),dest.q);
00147 Divide(src.qdot,factor.t,dest.qdot);
00148 Subtract(dest.qdot,dest.q,dest.qdot);
00149 Divide(src.q,factor.t,dest.q);
00150 }
00151 void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest)
00152 {
00153 Multiply(src.q,(2*factor.d*factor.d)/(factor.t*factor.t*factor.t)-factor.dd/(factor.t*factor.t),dest.q);
00154 Multiply(src.qdot,(2*factor.d)/(factor.t*factor.t),dest.qdot);
00155 Divide(src.qdotdot,factor.t,dest.qdotdot);
00156 Subtract(dest.qdotdot,dest.qdot,dest.qdotdot);
00157 Add(dest.qdotdot,dest.q,dest.qdotdot);
00158 Multiply(src.q,factor.d/(factor.t*factor.t),dest.q);
00159 Divide(src.qdot,factor.t,dest.qdot);
00160 Subtract(dest.qdot,dest.q,dest.qdot);
00161 Divide(src.q,factor.t,dest.q);
00162 }
00163
00164 void SetToZero(JntArrayAcc& array)
00165 {
00166 SetToZero(array.q);
00167 SetToZero(array.qdot);
00168 SetToZero(array.qdotdot);
00169 }
00170
00171 bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps)
00172 {
00173 return (Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps)&&Equal(src1.qdotdot,src2.qdotdot,eps));
00174 }
00175 }
00176
00177