public static RigidBodyTransform createTransformFromTranslationAndEulerAngles(double x, double y, double z, double roll, double pitch, double yaw) { return createTransformFromTranslationAndEulerAngles(new Vector3d(x, y, z), new Vector3d(roll, pitch, yaw)); }
public static RigidBodyTransform createTransformFromTranslationAndEulerAngles(double x, double y, double z, double roll, double pitch, double yaw) { return createTransformFromTranslationAndEulerAngles(new Vector3D(x, y, z), new Vector3D(roll, pitch, yaw)); }
private boolean yawBigInDoubleSupport(ExternalPelvisPoseCreator externalPelvisPoseCreator) throws SimulationExceededMaximumTimeException { long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); RigidBodyTransform yawTransform = TransformTools.createTransformFromTranslationAndEulerAngles(1, 1, 0.8, 0, 0, Math.PI); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(yawTransform, timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1); externalPelvisPoseCreator.setNewestPose(posePacket); return drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(10); }
private boolean yawBigInSingleSupport(ExternalPelvisPoseCreator externalPelvisPoseCreator) throws SimulationExceededMaximumTimeException { // FootPosePacket packet = new FootPosePacket(RobotSide.RIGHT, new Point3D(1, 1, 0.3), new Quat4d(), 0.6); // drcSimulationTestHelper.send(packet); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(2); long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); RigidBodyTransform yawTransform = TransformTools.createTransformFromTranslationAndEulerAngles(1, 1, 0.8, 0, 0, Math.PI); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(yawTransform, timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1); externalPelvisPoseCreator.setNewestPose(posePacket); return drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(10); }
private boolean localizeOutsideOfFootInSingleSupport(ExternalPelvisPoseCreator externalPelvisPoseCreator) throws SimulationExceededMaximumTimeException { // FootPosePacket packet = new FootPosePacket(RobotSide.RIGHT, new Point3D(1.0, 1.0, 0.3), new Quat4d(), 0.6); // drcSimulationTestHelper.send(packet); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(2.0); long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); RigidBodyTransform outsideOfFootTransform = TransformTools.createTransformFromTranslationAndEulerAngles(1.5, 1.0, 0.8, 0.0, 0.0, 0.0); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(outsideOfFootTransform, timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0); externalPelvisPoseCreator.setNewestPose(posePacket); return drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(10.0); }