public void variableChanged(YoVariable<?> v) { if (legInverseKinematicsCalculators == null) return; reader.read(); sdfRobot.update(); if (sliderSpace.getEnumValue() == SliderSpace.CARTESIAN) { for (RobotSide robotSide : RobotSide.values()) { YoFramePose footIK = feetIKs.get(robotSide); FramePoint position = footIK.getPosition().getFramePointCopy(); FrameOrientation orientation = footIK.getOrientation().getFrameOrientationCopy(); FramePose framePose = new FramePose(position, orientation); framePose.changeFrame(fullRobotModel.getPelvis().getBodyFixedFrame()); framePose.getPose(desiredTransform); legInverseKinematicsCalculators.get(robotSide).solve(desiredTransform); YoFramePose handIK = handIKs.get(robotSide); position = handIK.getPosition().getFramePointCopy(); orientation = handIK.getOrientation().getFrameOrientationCopy(); framePose = new FramePose(position, orientation); framePose.changeFrame(fullRobotModel.getChest().getBodyFixedFrame()); framePose.getPose(desiredTransform); armInverseKinematicsCalculators.get(robotSide).solve(desiredTransform); } writer.updateRobotConfigurationBasedOnFullRobotModel(); } } }
public void variableChanged(YoVariable<?> v) { if (legInverseKinematicsCalculators == null) return; reader.read(); sdfRobot.update(); if (sliderSpace.getEnumValue() == SliderSpace.CARTESIAN) { for (RobotSide robotSide : RobotSide.values()) { YoFramePose footIK = feetIKs.get(robotSide); FramePoint position = footIK.getPosition().getFramePointCopy(); FrameOrientation orientation = footIK.getOrientation().getFrameOrientationCopy(); FramePose framePose = new FramePose(position, orientation); framePose.changeFrame(fullRobotModel.getPelvis().getBodyFixedFrame()); framePose.getPose(desiredTransform); legInverseKinematicsCalculators.get(robotSide).solve(desiredTransform); YoFramePose handIK = handIKs.get(robotSide); position = handIK.getPosition().getFramePointCopy(); orientation = handIK.getOrientation().getFrameOrientationCopy(); framePose = new FramePose(position, orientation); framePose.changeFrame(fullRobotModel.getChest().getBodyFixedFrame()); framePose.getPose(desiredTransform); armInverseKinematicsCalculators.get(robotSide).solve(desiredTransform); } writer.updateRobotConfigurationBasedOnFullRobotModel(); } } }