InvKinemaResolver.java
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  * General Robotix Inc.
8  * National Institute of Advanced Industrial Science and Technology (AIST)
9  */
17 package com.generalrobotix.ui.view.tdview;
18 
19 import javax.media.j3d.Transform3D;
20 import javax.media.j3d.TransformGroup;
21 import javax.vecmath.*;
22 
23 import jp.go.aist.hrp.simulator.*;
24 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.*;
25 
29 
34 public class InvKinemaResolver {
35  private DynamicsSimulator integrator_;
38  private GrxLinkItem from_;
39  private GrxLinkItem to_;
40  private Transform3D trFrom_;
41  private LinkPosition tr;
42 
51  manager_ = manager;
52  trFrom_ = new Transform3D();
53  tr = new LinkPosition();
54  tr.p = new double[3];
55  tr.R = new double[9];
56  }
57 
58  public void setDynamicsSimulator(DynamicsSimulator integrator) {
59  integrator_ = integrator;
60  }
61 
69  public boolean setFromJoint(GrxModelItem model, GrxLinkItem link) {
70  if (robot_ == null) {
71  robot_ = model;
72  } else if(robot_ != model) {
73  to_ = null;
74  robot_ = model;
75  }
76 
77  from_ = link;
78 
79  // fromジョイントのグローバル座標での位置姿勢を保持
80  TransformGroup tg = from_.tg_;
81  Transform3D tr = new Transform3D();
82  tg.getTransform(tr);
83  tg.getLocalToVworld(trFrom_);
84  trFrom_.mul(tr);
85  //tg.getTransform(trFrom_);
86 
87  return true;
88  }
89 
97  public boolean setToJoint(GrxModelItem model, GrxLinkItem link) {
98  if (robot_ == null) {
99  robot_ = model;
100  } else if(robot_ != model) {
101  return false;
102  }
103 
104  to_ = link;
105 
106  return true;
107  }
108 
115  private void _setLinkStatus(String objectName) {
116  if(robot_ == null)
117  robot_ = (GrxModelItem)manager_.getItem(GrxModelItem.class, objectName);
118 
119  TransformGroup tg = robot_.getTransformGroupRoot();
120  Transform3D t3d = new Transform3D();
121  tg.getTransform(t3d);
122  Vector3d pos = new Vector3d();
123  Matrix3d mat = new Matrix3d();
124  t3d.get(mat, pos);
125 
126  double[] value = new double[12];
127  pos.get(value);
128  value[3] = mat.m00;
129  value[4] = mat.m01;
130  value[5] = mat.m02;
131  value[6] = mat.m10;
132  value[7] = mat.m11;
133  value[8] = mat.m12;
134  value[9] = mat.m20;
135  value[10]= mat.m21;
136  value[11]= mat.m22;
137 
138  integrator_.setCharacterLinkData(
139  objectName, robot_.rootLink().getName(), LinkDataType.ABS_TRANSFORM, value
140  );
141  integrator_.setCharacterAllLinkData(
142  objectName, LinkDataType.JOINT_VALUE, robot_.getJointValues()
143  );
144  integrator_.calcCharacterForwardKinematics(objectName);
145  }
146 
155  public boolean resolve(Transform3D transform) {
156  _setLinkStatus(robot_.getName());
157 
158  Matrix3d m3d = new Matrix3d();
159  Vector3d v3d = new Vector3d();
160  transform.get(m3d, v3d);
161 
162  v3d.get(tr.p);
163  for (int i=0; i<3; i++) {
164  for (int j=0; j<3; j++) {
165  tr.R[3*i+j] = m3d.getElement(i,j);
166  }
167  }
168  try {
169  if (robot_ == null || from_ == null || to_ == null)
170  return false;
171 
172  if (!integrator_.calcCharacterInverseKinematics(robot_.getName(), from_.getName(), to_.getName(), tr)) {
173  System.out.println("ik failed.");
174  robot_.calcForwardKinematics();
175  return false;
176  };
177  } catch (Exception e) {
178  e.printStackTrace();
179  }
180 
181  DblSequenceHolder v = new DblSequenceHolder();
182  integrator_.getCharacterAllLinkData(robot_.getName(), LinkDataType.JOINT_VALUE, v);
183  robot_.setJointValues(v.value);
184  robot_.setJointValuesWithinLimit();
185  robot_.updateInitialJointValues();
186  _setRootJoint(robot_);
187  robot_.calcForwardKinematics();
188 
189  return true;
190  }
191 
192  private void _setRootJoint(GrxModelItem robot) {
193  Transform3D t3d = new Transform3D();
194  from_.tg_.getTransform(t3d);
195 
196  Transform3D t3dFromNew = new Transform3D();
197  from_.tg_.getLocalToVworld(t3dFromNew);
198  t3dFromNew.mul(t3d);
199  t3dFromNew.invert();
200 
201  Transform3D t3dRoot = new Transform3D();
202  robot.getTransformGroupRoot().getTransform(t3dRoot);
203  t3d.mul(trFrom_, t3dFromNew);
204  t3d.mul(t3dRoot);
205 
206  Matrix3d rot = new Matrix3d();
207  Vector3d pos = new Vector3d();
208  t3d.get(rot, pos);
209  robot.setTransformRoot(pos, rot);
211  /*
212  AxisAngle4d a4d = new AxisAngle4d();
213  a4d.set(rot);
214  robot.setProperty(robot_.lInfo_[0].name+".rotation", +a4d.angle+" "+a4d.x+" "+a4d.y+" "+a4d.z);
215  robot.setProperty(robot_.lInfo_[0].name+".translation", pos.x+" "+pos.y+" "+pos.z);*/
216  }
217 }
void setJointValuesWithinLimit()
modify joint value if it exceeds limit values
void updateInitialTransformRoot()
update initial transformation property from current Transform3D
#define null
our own NULL pointer
Definition: IceTypes.h:57
boolean setToJoint(GrxModelItem model, GrxLinkItem link)
png_voidp int value
Definition: png.h:2113
item corresponds to a robot model
png_uint_32 i
Definition: png.h:2735
boolean setFromJoint(GrxModelItem model, GrxLinkItem link)
GrxLinkItem rootLink()
get root link
void setTransformRoot(Vector3d pos, Matrix3d rot)
set transformation of the root joint
GrxBaseItem getItem(Class<? extends GrxBaseItem > cls, String name)
void calcForwardKinematics()
compute forward kinematics
void setJointValues(final double[] values)
set joint values
プラグイン管理クラス GrxUIの核になるクラス。プラグインのロード等の、初期化を実行する。 プラグインとそ...
void setDynamicsSimulator(DynamicsSimulator integrator)


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:04