private void _setRootJoint(GrxModelItem robot) {
Transform3D t3d = new Transform3D();
from_.tg_.getTransform(t3d);
Transform3D t3dFromNew = new Transform3D();
from_.tg_.getLocalToVworld(t3dFromNew);
t3dFromNew.mul(t3d);
t3dFromNew.invert();
Transform3D t3dRoot = new Transform3D();
robot.getTransformGroupRoot().getTransform(t3dRoot);
t3d.mul(trFrom_, t3dFromNew);
t3d.mul(t3dRoot);
Matrix3d rot = new Matrix3d();
Vector3d pos = new Vector3d();
t3d.get(rot, pos);
robot.setTransformRoot(pos, rot);
robot.updateInitialTransformRoot();
/*
AxisAngle4d a4d = new AxisAngle4d();
a4d.set(rot);
robot.setProperty(robot_.lInfo_[0].name+".rotation", +a4d.angle+" "+a4d.x+" "+a4d.y+" "+a4d.z);
robot.setProperty(robot_.lInfo_[0].name+".translation", pos.x+" "+pos.y+" "+pos.z);*/
}
/**
* setLinkStatus
*
* <p>Debugようにこのメソッドを叩くと現在のRobotの状態をIntegratorに通知する
*
* @param objectName
*/
private void _setLinkStatus(String objectName) {
if (robot_ == null) robot_ = (GrxModelItem) manager_.getItem(GrxModelItem.class, objectName);
TransformGroup tg = robot_.getTransformGroupRoot();
Transform3D t3d = new Transform3D();
tg.getTransform(t3d);
Vector3d pos = new Vector3d();
Matrix3d mat = new Matrix3d();
t3d.get(mat, pos);
double[] value = new double[12];
pos.get(value);
value[3] = mat.m00;
value[4] = mat.m01;
value[5] = mat.m02;
value[6] = mat.m10;
value[7] = mat.m11;
value[8] = mat.m12;
value[9] = mat.m20;
value[10] = mat.m21;
value[11] = mat.m22;
integrator_.setCharacterLinkData(
objectName, robot_.rootLink().getName(), LinkDataType.ABS_TRANSFORM, value);
integrator_.setCharacterAllLinkData(
objectName, LinkDataType.JOINT_VALUE, robot_.getJointValues());
integrator_.calcCharacterForwardKinematics(objectName);
}
/**
* resolve
*
* <p>マウスの動きから生成された T3D を逆運動学サーバを使って 各ジョイントの動きに直し設定する
*
* @param transform
* @return
*/
public boolean resolve(Transform3D transform) {
_setLinkStatus(robot_.getName());
Matrix3d m3d = new Matrix3d();
Vector3d v3d = new Vector3d();
transform.get(m3d, v3d);
v3d.get(tr.p);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
tr.R[3 * i + j] = m3d.getElement(i, j);
}
}
try {
if (robot_ == null || from_ == null || to_ == null) return false;
if (!integrator_.calcCharacterInverseKinematics(
robot_.getName(), from_.getName(), to_.getName(), tr)) {
System.out.println("ik failed.");
robot_.calcForwardKinematics();
return false;
}
;
} catch (Exception e) {
e.printStackTrace();
}
DblSequenceHolder v = new DblSequenceHolder();
integrator_.getCharacterAllLinkData(robot_.getName(), LinkDataType.JOINT_VALUE, v);
robot_.setJointValues(v.value);
robot_.setJointValuesWithinLimit();
robot_.updateInitialJointValues();
_setRootJoint(robot_);
robot_.calcForwardKinematics();
return true;
}
