private void updateDesiredFullRobotModelState() { RootJointDesiredConfigurationDataReadOnly outputForRootJoint = wholeBodyInverseKinematicsSolver.getOutputForRootJoint(); desiredRootJoint.setConfiguration(outputForRootJoint.getDesiredConfiguration(), 0); desiredRootJoint.setVelocity(outputForRootJoint.getDesiredVelocity(), 0); LowLevelOneDoFJointDesiredDataHolderReadOnly output = wholeBodyInverseKinematicsSolver.getOutput(); for (OneDoFJoint joint : oneDoFJoints) { if (output.hasDataForJoint(joint)) { joint.setQ(output.getDesiredJointPosition(joint)); joint.setqDesired(output.getDesiredJointPosition(joint)); joint.setQd(output.getDesiredJointVelocity(joint)); } } }
private void updateDesiredFullRobotModelState() { RootJointDesiredConfigurationDataReadOnly outputForRootJoint = wholeBodyInverseKinematicsSolver.getOutputForRootJoint(); desiredRootJoint.setConfiguration(outputForRootJoint.getDesiredConfiguration(), 0); desiredRootJoint.setVelocity(outputForRootJoint.getDesiredVelocity(), 0); LowLevelOneDoFJointDesiredDataHolderReadOnly output = wholeBodyInverseKinematicsSolver.getOutput(); for (OneDoFJoint joint : oneDoFJoints) { if (output.hasDataForJoint(joint)) { joint.setQ(output.getDesiredJointPosition(joint)); joint.setqDesired(output.getDesiredJointPosition(joint)); joint.setQd(output.getDesiredJointVelocity(joint)); } } }
Quat4f orientation = robotConfigurationData.getPelvisOrientation(); desiredRootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); desiredRootJoint.setVelocity(zeroVelocityMatrix, 0);
Quat4f orientation = robotConfigurationData.getPelvisOrientation(); desiredRootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); desiredRootJoint.setVelocity(zeroVelocityMatrix, 0);