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