com.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  * handle.cpp
11  * Create: Katsu Yamane, 03.07.10
12  */
13 
14 #include "ik.h"
15 
17 {
18  if(n_const == 0) return 0;
19  int n_dof = ik->NumDOF();
20  fMat Jtemp;
21  J.resize(n_const, n_dof);
22  Jtemp.resize(3, n_dof);
23  Jtemp.zero();
24  ik->ComJacobian(Jtemp, cur_com, charname);
25  int i, j, count = 0;
26  for(i=0; i<3; i++)
27  {
29  {
30  for(j=0; j<n_dof; j++)
31  J(count, j) = Jtemp(i, j);
32  count++;
33  }
34  }
35  return 0;
36 }
37 
39 {
40  int i, count = 0;
41  fVec3 dp;
42  dp.sub(des_com, cur_com);
43  dp *= gain;
44  for(i=0; i<3; i++)
45  {
47  {
48  fb(count) = dp(i);
49  count++;
50  }
51  }
52  return 0;
53 }
54 
char * charname
target character name
Definition: ik.h:704
int resize(int i, int j)
Changes the matrix size.
Definition: fMatrix.h:133
fVec3 des_com
desired COM position
Definition: ik.h:705
Inverse kinematics (UTPoser) class.
double gain
priority
Definition: ik.h:396
png_uint_32 i
Definition: png.h:2735
int calc_feedback()
compute the feedback velocity
Definition: com.cpp:38
with constraint
Definition: ik.h:67
IK::ConstIndex const_index[3]
which of three directions (x, y, z) are constrained
Definition: ik.h:703
void zero()
Creates a zero matrix.
Definition: fMatrix.h:249
def j(str, encoding="cp932")
int NumDOF()
Total degrees of freedom.
Definition: chain.h:343
fMat J
Definition: ik.h:402
fVec fb
Jacobian matrix (n_const x total DOF)
Definition: ik.h:403
int calc_jacobian()
Computes the constraint Jacobian.
Definition: com.cpp:16
double ComJacobian(fMat &J, fVec3 &com, const char *chname=0)
Computes the com Jacobian.
Definition: jacobi.cpp:18
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
fVec3 cur_com
current COM position
Definition: ik.h:706
IK * ik
Definition: ik.h:390
3-element vector class.
Definition: fMatrix3.h:206
int n_const
weight
Definition: ik.h:398
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order...
Definition: fMatrix.h:46


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