desire.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  * desire.cpp
00011  * Create: Katsu Yamane, 03.07.10
00012  */
00013 
00014 #include "ik.h"
00015 
00016 int IKDesire::calc_jacobian_rotate(Joint* cur)
00017 {
00018         if(cur == joint && cur->t_given)
00019         {
00020                 J(0, cur->i_dof) = 1.0;
00021         }
00022         return 0;
00023 }
00024 
00025 int IKDesire::calc_jacobian_slide(Joint* cur)
00026 {
00027         if(cur == joint && cur->t_given)
00028         {
00029                 J(0, cur->i_dof) = 1.0;
00030         }
00031         return 0;
00032 }
00033 
00034 int IKDesire::calc_jacobian_sphere(Joint* cur)
00035 {
00036         if(cur == joint && cur->t_given)
00037         {
00038                 for(int i=0; i<3; i++)
00039                 {
00040                         J(0, cur->i_dof+i) = cur->rel_att(0, i);
00041                         J(1, cur->i_dof+i) = cur->rel_att(1, i);
00042                         J(2, cur->i_dof+i) = cur->rel_att(2, i);
00043                 }
00044         }
00045         return 0;
00046 }
00047 
00048 int IKDesire::calc_jacobian_free(Joint* cur)
00049 {
00050         if(cur == joint && cur->t_given)
00051         {
00052                 for(int i=0; i<3; i++)
00053                 {
00054                         J(0, cur->i_dof+i) = cur->rel_att(0, i);
00055                         J(1, cur->i_dof+i) = cur->rel_att(1, i);
00056                         J(2, cur->i_dof+i) = cur->rel_att(2, i);
00057                         J(3, cur->i_dof+i+3) = cur->rel_att(0, i);
00058                         J(4, cur->i_dof+i+3) = cur->rel_att(1, i);
00059                         J(5, cur->i_dof+i+3) = cur->rel_att(2, i);
00060                 }
00061         }
00062         return 0;
00063 }
00064 
00065 int IKDesire::calc_feedback()
00066 {
00067         double q_cur;
00068         fVec3 fb_pos, fb_att;
00069         switch(joint->j_type)
00070         {
00071         case JROTATE:
00072         case JSLIDE:
00073                 joint->GetJointValue(q_cur);
00074                 fb(0) = gain * (q_des - q_cur);
00075                 break;
00076         case JSPHERE:
00077                 fb_att.rotation(att_des, joint->rel_att);
00078                 fb_att *= gain;
00079                 fb(0) = fb_att(0);
00080                 fb(1) = fb_att(1);
00081                 fb(2) = fb_att(2);
00082                 break;
00083         case JFREE:
00084                 fb_pos.sub(pos_des, joint->rel_pos);
00085                 fb_pos *= gain;
00086                 fb_att.rotation(att_des, joint->rel_att);
00087                 fb_att *= gain;
00088                 fb(0) = fb_pos(0);
00089                 fb(1) = fb_pos(1);
00090                 fb(2) = fb_pos(2);
00091                 fb(3) = fb_att(0);
00092                 fb(4) = fb_att(1);
00093                 fb(5) = fb_att(2);
00094                 break;
00095         default:
00096                 break;
00097         }
00098         return 0;
00099 }


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