jntarrayvel.cpp
Go to the documentation of this file.
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 
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 


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22