/**
* 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);
}
