00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "jntarrayacc.hpp"
00024
00025 namespace KDL
00026 {
00027 JntArrayVel::JntArrayVel(unsigned int size):
00028 q(size),qdot(size)
00029 {
00030 }
00031 JntArrayVel::JntArrayVel(const JntArray& qin, const JntArray& qdotin):
00032 q(qin),qdot(qdotin)
00033 {
00034 assert(q.rows()==qdot.rows());
00035 }
00036 JntArrayVel::JntArrayVel(const JntArray& qin):
00037 q(qin),qdot(q.rows())
00038 {
00039 }
00040
00041 void JntArrayVel::resize(unsigned int newSize)
00042 {
00043 q.resize(newSize);
00044 qdot.resize(newSize);
00045 }
00046
00047 JntArray JntArrayVel::value()const
00048 {
00049 return q;
00050 }
00051
00052 JntArray JntArrayVel::deriv()const
00053 {
00054 return qdot;
00055 }
00056
00057 void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest)
00058 {
00059 Add(src1.q,src2.q,dest.q);
00060 Add(src1.qdot,src2.qdot,dest.qdot);
00061 }
00062 void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest)
00063 {
00064 Add(src1.q,src2,dest.q);
00065 dest.qdot=src1.qdot;
00066 }
00067
00068 void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest)
00069 {
00070 Subtract(src1.q,src2.q,dest.q);
00071 Subtract(src1.qdot,src2.qdot,dest.qdot);
00072 }
00073 void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest)
00074 {
00075 Subtract(src1.q,src2,dest.q);
00076 dest.qdot=src1.qdot;
00077 }
00078
00079 void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest)
00080 {
00081 Multiply(src.q,factor,dest.q);
00082 Multiply(src.qdot,factor,dest.qdot);
00083 }
00084 void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest)
00085 {
00086 Multiply(src.q,factor.grad,dest.q);
00087 Multiply(src.qdot,factor.t,dest.qdot);
00088 Add(dest.qdot,dest.q,dest.qdot);
00089 Multiply(src.q,factor.t,dest.q);
00090 }
00091
00092 void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest)
00093 {
00094 Divide(src.q,factor,dest.q);
00095 Divide(src.qdot,factor,dest.qdot);
00096 }
00097 void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest)
00098 {
00099 Multiply(src.q,(factor.grad/factor.t/factor.t),dest.q);
00100 Divide(src.qdot,factor.t,dest.qdot);
00101 Subtract(dest.qdot,dest.q,dest.qdot);
00102 Divide(src.q,factor.t,dest.q);
00103 }
00104
00105 void SetToZero(JntArrayVel& array)
00106 {
00107 SetToZero(array.q);
00108 SetToZero(array.qdot);
00109 }
00110
00111 bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps)
00112 {
00113 return Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps);
00114 }
00115 }
00116
00117