com.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00009 /*
00010  * handle.cpp
00011  * Create: Katsu Yamane, 03.07.10
00012  */
00013 
00014 #include "ik.h"
00015 
00016 int IKCom::calc_jacobian()
00017 {
00018         if(n_const == 0) return 0;
00019         int n_dof = ik->NumDOF();
00020         fMat Jtemp;
00021         J.resize(n_const, n_dof);
00022         Jtemp.resize(3, n_dof);
00023         Jtemp.zero();
00024         ik->ComJacobian(Jtemp, cur_com, charname);
00025         int i, j, count = 0;
00026         for(i=0; i<3; i++)
00027         {
00028                 if(const_index[i] == IK::HAVE_CONSTRAINT)
00029                 {
00030                         for(j=0; j<n_dof; j++)
00031                                 J(count, j) = Jtemp(i, j);
00032                         count++;
00033                 }
00034         }
00035         return 0;
00036 }
00037 
00038 int IKCom::calc_feedback()
00039 {
00040         int i, count = 0;
00041         fVec3 dp;
00042         dp.sub(des_com, cur_com);
00043         dp *= gain;
00044         for(i=0; i<3; i++)
00045         {
00046                 if(const_index[i] == IK::HAVE_CONSTRAINT)
00047                 {
00048                         fb(count) = dp(i);
00049                         count++;
00050                 }
00051         }
00052         return 0;
00053 }
00054 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53