limit.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * The University of Tokyo
8  */
14 #include <ik.h>
15 
17 {
18  if(cur == joint && cur->j_type == JROTATE && cur->t_given)
19  {
20  J(0, cur->i_dof) = 1.0;
21  }
22  return 0;
23 }
24 
26 {
27  if(cur == joint && cur->j_type == JSLIDE && cur->t_given)
28  {
29  J(0, cur->i_dof) = 1.0;
30  }
31  return 0;
32 }
33 
35 {
36  if(active && joint->n_dof == 1)
37  {
38  enabled = true;
39  double q_cur;
40  joint->GetJointValue(q_cur);
41  if(min_limit && q_cur < q_min)
42  {
43 // cerr << joint->name << " is below min (" << q_cur << " < " << q_min << ")" << endl;
44  fb(0) = gain * (q_min - q_cur);
45  }
46  else if(max_limit && q_cur > q_max)
47  {
48 // cerr << joint->name << " is above max (" << q_cur << " > " << q_max << ")" << endl;
49  fb(0) = gain * (q_max - q_cur);
50  }
51  }
52  else
53  {
54  enabled = false;
55  }
56  return 0;
57 }
58 
59 void IKScalarJointLimit::SetCharacterScale(double _scale, const char* charname)
60 {
61 }
62 
double q_max
Definition: ik.h:771
int min_limit
Definition: ik.h:770
int max_limit
Definition: ik.h:770
int n_dof
degrees of freedom (0/1/3/6)
Definition: chain.h:715
int t_given
torque or motion controlled
Definition: chain.h:709
Inverse kinematics (UTPoser) class.
void SetCharacterScale(double _scale, const char *charname=0)
Definition: limit.cpp:59
int calc_jacobian_slide(Joint *cur)
Definition: limit.cpp:25
double gain
priority
Definition: ik.h:396
int active
Definition: ik.h:400
int calc_jacobian_rotate(Joint *cur)
Definition: limit.cpp:16
int GetJointValue(double &_q)
Definition: joint.cpp:256
int i_dof
index in all DOF
Definition: chain.h:720
Joint * joint
target joint
Definition: ik.h:391
JointType j_type
joint type
Definition: chain.h:694
fMat J
Definition: ik.h:402
fVec fb
Jacobian matrix (n_const x total DOF)
Definition: ik.h:403
int enabled
number of constraints
Definition: ik.h:399
prismatic (1DOF)
Definition: chain.h:42
int calc_feedback()
compute the feedback velocity
Definition: limit.cpp:34
double q_min
Definition: ik.h:771
rotational (1DOF)
Definition: chain.h:41
The class for representing a joint.
Definition: chain.h:538


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:23