desire.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  */
9 /*
10  * desire.cpp
11  * Create: Katsu Yamane, 03.07.10
12  */
13 
14 #include "ik.h"
15 
17 {
18  if(cur == joint && cur->t_given)
19  {
20  J(0, cur->i_dof) = 1.0;
21  }
22  return 0;
23 }
24 
26 {
27  if(cur == joint && cur->t_given)
28  {
29  J(0, cur->i_dof) = 1.0;
30  }
31  return 0;
32 }
33 
35 {
36  if(cur == joint && cur->t_given)
37  {
38  for(int i=0; i<3; i++)
39  {
40  J(0, cur->i_dof+i) = cur->rel_att(0, i);
41  J(1, cur->i_dof+i) = cur->rel_att(1, i);
42  J(2, cur->i_dof+i) = cur->rel_att(2, i);
43  }
44  }
45  return 0;
46 }
47 
49 {
50  if(cur == joint && cur->t_given)
51  {
52  for(int i=0; i<3; i++)
53  {
54  J(0, cur->i_dof+i) = cur->rel_att(0, i);
55  J(1, cur->i_dof+i) = cur->rel_att(1, i);
56  J(2, cur->i_dof+i) = cur->rel_att(2, i);
57  J(3, cur->i_dof+i+3) = cur->rel_att(0, i);
58  J(4, cur->i_dof+i+3) = cur->rel_att(1, i);
59  J(5, cur->i_dof+i+3) = cur->rel_att(2, i);
60  }
61  }
62  return 0;
63 }
64 
66 {
67  double q_cur;
68  fVec3 fb_pos, fb_att;
69  switch(joint->j_type)
70  {
71  case JROTATE:
72  case JSLIDE:
73  joint->GetJointValue(q_cur);
74  fb(0) = gain * (q_des - q_cur);
75  break;
76  case JSPHERE:
77  fb_att.rotation(att_des, joint->rel_att);
78  fb_att *= gain;
79  fb(0) = fb_att(0);
80  fb(1) = fb_att(1);
81  fb(2) = fb_att(2);
82  break;
83  case JFREE:
84  fb_pos.sub(pos_des, joint->rel_pos);
85  fb_pos *= gain;
86  fb_att.rotation(att_des, joint->rel_att);
87  fb_att *= gain;
88  fb(0) = fb_pos(0);
89  fb(1) = fb_pos(1);
90  fb(2) = fb_pos(2);
91  fb(3) = fb_att(0);
92  fb(4) = fb_att(1);
93  fb(5) = fb_att(2);
94  break;
95  default:
96  break;
97  }
98  return 0;
99 }
void rotation(const fMat33 &ref, const fMat33 &tgt)
Computes the rotation from tgt to ref.
Definition: fMatrix3.cpp:954
int t_given
torque or motion controlled
Definition: chain.h:709
Inverse kinematics (UTPoser) class.
double gain
priority
Definition: ik.h:396
int calc_jacobian_sphere(Joint *cur)
Definition: desire.cpp:34
fVec3 pos_des
desired joint position for 6DOF joints
Definition: ik.h:647
png_uint_32 i
Definition: png.h:2735
double q_des
desired joint value for 1DOF joints
Definition: ik.h:645
fMat33 att_des
desired joint orientation for 3/6 DOF joints
Definition: ik.h:646
spherical (3DOF)
Definition: chain.h:43
fVec3 rel_pos
(initial) position in parent joint&#39;s frame (for 0/3/6 DOF joints)
Definition: chain.h:700
int GetJointValue(double &_q)
Definition: joint.cpp:256
int i_dof
index in all DOF
Definition: chain.h:720
int calc_jacobian_rotate(Joint *cur)
Definition: desire.cpp:16
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
prismatic (1DOF)
Definition: chain.h:42
fMat33 rel_att
(initial) orientation in parent joint&#39;s frame (for 0/3/6 DOF joints)
Definition: chain.h:701
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
int calc_feedback()
compute the feedback velocity
Definition: desire.cpp:65
3-element vector class.
Definition: fMatrix3.h:206
rotational (1DOF)
Definition: chain.h:41
The class for representing a joint.
Definition: chain.h:538
int calc_jacobian_slide(Joint *cur)
Definition: desire.cpp:25
free (6DOF)
Definition: chain.h:44
int calc_jacobian_free(Joint *cur)
Definition: desire.cpp:48


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