limit.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  */
00014 #include <ik.h>
00015 
00016 int IKScalarJointLimit::calc_jacobian_rotate(Joint* cur)
00017 {
00018         if(cur == joint && cur->j_type == JROTATE && cur->t_given)
00019         {
00020                 J(0, cur->i_dof) = 1.0;
00021         }
00022         return 0;
00023 }
00024 
00025 int IKScalarJointLimit::calc_jacobian_slide(Joint* cur)
00026 {
00027         if(cur == joint && cur->j_type == JSLIDE && cur->t_given)
00028         {
00029                 J(0, cur->i_dof) = 1.0;
00030         }
00031         return 0;
00032 }
00033 
00034 int IKScalarJointLimit::calc_feedback()
00035 {
00036         if(active && joint->n_dof == 1)
00037         {
00038                 enabled = true;
00039                 double q_cur;
00040                 joint->GetJointValue(q_cur);
00041                 if(min_limit && q_cur < q_min)
00042                 {
00043 //                      cerr << joint->name << " is below min (" << q_cur << " < " << q_min << ")" << endl;
00044                         fb(0) = gain * (q_min - q_cur);
00045                 }
00046                 else if(max_limit && q_cur > q_max)
00047                 {
00048 //                      cerr << joint->name << " is above max (" << q_cur << " > " << q_max << ")" << endl;
00049                         fb(0) = gain * (q_max - q_cur);
00050                 }
00051         }
00052         else
00053         {
00054                 enabled = false;
00055         }
00056         return 0;
00057 }
00058 
00059 void IKScalarJointLimit::SetCharacterScale(double _scale, const char* charname)
00060 {
00061 }
00062 


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:55