$search
00001 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00002 00003 // Version: 1.0 00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00006 // URL: http://www.orocos.org/kdl 00007 00008 // This library is free software; you can redistribute it and/or 00009 // modify it under the terms of the GNU Lesser General Public 00010 // License as published by the Free Software Foundation; either 00011 // version 2.1 of the License, or (at your option) any later version. 00012 00013 // This library is distributed in the hope that it will be useful, 00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00016 // Lesser General Public License for more details. 00017 00018 // You should have received a copy of the GNU Lesser General Public 00019 // License along with this library; if not, write to the Free Software 00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 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