public static Footstep[] createFootsteps(int numberOfSteps) { Footstep[] footsteps = new Footstep[numberOfSteps]; for (int i = 0; i < numberOfSteps; i++) { footsteps[i] = new Footstep(); } return footsteps; }
public FootstepTask(FullHumanoidRobotModel fullRobotModel, RobotSide robotSide, FootstepListBehavior footstepListBehavior, FramePose3D footPose) { super(footstepListBehavior); footsteps.add(new Footstep(robotSide, footPose)); this.footstepListBehavior = footstepListBehavior; }
public static Footstep generateStandingFootstep(RobotSide side, RigidBodyBasics foot, ReferenceFrame soleFrame) { FramePose3D footFramePose = new FramePose3D(soleFrame); footFramePose.changeFrame(worldFrame); boolean trustHeight = false; Footstep footstep = new Footstep(side, footFramePose, trustHeight); return footstep; }
private ArrayList<Footstep> createRandomFootsteps(int number) { Random random = new Random(77); ArrayList<Footstep> footsteps = new ArrayList<Footstep>(); for (int footstepNumber = 0; footstepNumber < number; footstepNumber++) { FramePose3D pose = new FramePose3D(ReferenceFrame.getWorldFrame(), new Point3D(footstepNumber, 0.0, 0.0), new Quaternion(random.nextDouble(), random.nextDouble(), random.nextDouble(), random.nextDouble())); boolean trustHeight = true; Footstep footstep = new Footstep(robotSide, pose, trustHeight); footsteps.add(footstep); } return footsteps; }
public FootstepTask(E stateEnum,FullHumanoidRobotModel fullRobotModel, RobotSide robotSide, FootstepListBehavior footstepListBehavior, FramePose footPose) { super(stateEnum, footstepListBehavior); ReferenceFrame soleFrame; RigidBody endEffector; soleFrame = fullRobotModel.getSoleFrame(robotSide); endEffector = fullRobotModel.getEndEffector(robotSide, LimbName.LEG); PoseReferenceFrame poseReferenceFrame = new PoseReferenceFrame("poseReferenceFrame", ReferenceFrame.getWorldFrame()); poseReferenceFrame.setPoseAndUpdate(footPose); footsteps.add(new Footstep(endEffector, robotSide, soleFrame, poseReferenceFrame)); this.footstepListBehavior = footstepListBehavior; }
private Footstep createFootsteps(double length, double width, int numberOfSteps) { ArrayList<Footstep> upcomingFootsteps = new ArrayList<>(); RobotSide robotSide = RobotSide.LEFT; for (int i = 0; i < numberOfSteps; i++) { FramePose3D footstepPose = new FramePose3D(ReferenceFrame.getWorldFrame()); footstepPose.setPosition(length * (i + 1), robotSide.negateIfRightSide(0.5 * width), 0.0); upcomingFootsteps.add(new Footstep(robotSide, footstepPose, false)); FramePoint2D referenceFootstepPosition = new FramePoint2D(footstepPose.getPosition()); robotSide = robotSide.getOppositeSide(); } return upcomingFootsteps.get(0); }
public static Footstep generateStandingFootstep(RobotSide side, SideDependentList<? extends ContactablePlaneBody> bipedFeet) { ContactablePlaneBody endEffector = bipedFeet.get(side); FramePose3D footFramePose = new FramePose3D(endEffector.getSoleFrame()); footFramePose.changeFrame(worldFrame); boolean trustHeight = false; Footstep footstep = new Footstep(side, footFramePose, trustHeight); return footstep; }
@Override public void receivedPacket(FootstepDataMessage packet) { FramePose3D pose = new FramePose3D(ReferenceFrame.getWorldFrame(), packet.getLocation(), packet.getOrientation()); boolean trustHeight = true; Footstep footstep = new Footstep(RobotSide.fromByte(packet.getRobotSide()), pose, trustHeight); reconstructedFootsteps.add(footstep); }
public static Footstep generateStandingFootstep(RobotSide side, RigidBody foot, ReferenceFrame soleFrame) { FramePose footFramePose = new FramePose(foot.getParentJoint().getFrameAfterJoint()); footFramePose.changeFrame(worldFrame); boolean trustHeight = false; PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footFramePose); Footstep footstep = new Footstep(foot, side, soleFrame, footstepPoseFrame, trustHeight); return footstep; }
@Override public void receivedPacket(FootstepDataListMessage packet) { boolean adjustable = packet.getAreFootstepsAdjustable(); List<FootstepDataMessage> footstepDataList = packet.getFootstepDataList(); for (int i = 0; i < footstepDataList.size(); i++) { FootstepDataMessage footstepData = footstepDataList.get(i); FramePose3D footstepPose = new FramePose3D(ReferenceFrame.getWorldFrame(), footstepData.getLocation(), footstepData.getOrientation()); Footstep footstep = new Footstep(robotSide, footstepPose, true, adjustable); footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(footstepData)); footstep.setTrajectoryType(TrajectoryType.fromByte(footstepData.getTrajectoryType())); footstep.setSwingHeight(footstepData.getSwingHeight()); reconstructedFootstepPath.add(footstep); } }
private void addFootstepDataList(FootstepDataListMessage footstepDataList) { ArrayList<FootstepDataMessage> footstepList = footstepDataList.getDataList(); ArrayList<Footstep> footsteps = new ArrayList<Footstep>(); for (FootstepDataMessage footstepData : footstepList) { RobotSide robotSide = footstepData.getRobotSide(); ContactablePlaneBody contactableBody = bipedFeet.get(robotSide); FramePose footstepPose = new FramePose(ReferenceFrame.getWorldFrame(), footstepData.getLocation(), footstepData.getOrientation()); PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footstepPose); String id = "scriptedFootstep_" + footstepCounter; Footstep footstep = new Footstep(id, contactableBody.getRigidBody(), robotSide, contactableBody.getSoleFrame(), footstepPoseFrame, true); footsteps.add(footstep); footstepCounter++; } footstepQueue.addAll(footsteps); }
public static Footstep generateStandingFootstep(RobotSide side, SideDependentList<? extends ContactablePlaneBody> bipedFeet) { ContactablePlaneBody endEffector = bipedFeet.get(side); FramePose footFramePose = new FramePose(endEffector.getFrameAfterParentJoint()); footFramePose.changeFrame(worldFrame); boolean trustHeight = false; PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footFramePose); Footstep footstep = new Footstep(endEffector.getRigidBody(), side, endEffector.getSoleFrame(), footstepPoseFrame, trustHeight); return footstep; }
private void addFootstepDataList(FootstepDataListMessage footstepDataList) { ArrayList<FootstepDataMessage> footstepList = footstepDataList.getDataList(); ArrayList<Footstep> footsteps = new ArrayList<Footstep>(); for (FootstepDataMessage footstepData : footstepList) { RobotSide robotSide = footstepData.getRobotSide(); ContactablePlaneBody contactableBody = bipedFeet.get(robotSide); FramePose footstepPose = new FramePose(ReferenceFrame.getWorldFrame(), footstepData.getLocation(), footstepData.getOrientation()); PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footstepPose); String id = "scriptedFootstep_" + footstepCounter; Footstep footstep = new Footstep(id, contactableBody.getRigidBody(), robotSide, contactableBody.getSoleFrame(), footstepPoseFrame, true); footsteps.add(footstep); footstepCounter++; } footstepQueue.addAll(footsteps); }
public Footstep createFootstep(RobotSide robotSide, FramePose footstepPose) { RigidBody foot = contactableFeet.get(robotSide).getRigidBody(); ReferenceFrame soleFrame = contactableFeet.get(robotSide).getSoleFrame(); Footstep ret = new Footstep(foot, robotSide, soleFrame); ret.setPose(footstepPose); ret.setPredictedContactPointsFromFramePoint2ds(contactableFeet.get(robotSide).getContactPoints2d()); return ret; } }
private Footstep generateFootstep(FramePose2D footPose2d, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, double height, Vector3D planeNormal) { double yaw = footPose2d.getYaw(); Point3D position = new Point3D(footPose2d.getX(), footPose2d.getY(), height); Quaternion orientation = new Quaternion(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); FramePose3D footstepPose = new FramePose3D(ReferenceFrame.getWorldFrame(), position, orientation); Footstep footstep = new Footstep(robotSide, footstepPose); return footstep; }
@Override public Footstep generateFootstepWithoutHeightMap(FramePose2D footPose2d, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, double height, Vector3D planeNormal) { double yaw = footPose2d.getYaw(); Point3D position = new Point3D(footPose2d.getX(), footPose2d.getY(), height); Quaternion orientation = new Quaternion(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); FramePose3D solePose = new FramePose3D(ReferenceFrame.getWorldFrame(), position, orientation); Footstep footstep = new Footstep(robotSide, solePose); return footstep; }
private Footstep generateFootstepFromLocationAndOrientation(RobotSide robotSide, double[] positionArray, double[] orientationArray) { Footstep footstep = new Footstep(robotSide); Point3D position = new Point3D(positionArray); Quaternion orientation = new Quaternion(orientationArray); RigidBodyTransform anklePose = new RigidBodyTransform(); anklePose.setRotation(orientation); anklePose.setTranslation(position); FramePose3D pose = new FramePose3D(ReferenceFrame.getWorldFrame(), anklePose); footstep.setFromAnklePose(pose, transformsFromAnkleToSole.get(robotSide)); return footstep; }
@Override public Footstep generateFootstepWithoutHeightMap(FramePose2d footPose2d, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide, double height, Vector3d planeNormal) { double yaw = footPose2d.getYaw(); Point3d position = new Point3d(footPose2d.getX(), footPose2d.getY(), height); Quat4d orientation = new Quat4d(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); Footstep footstep = new Footstep(foot, robotSide, soleFrame); footstep.setSolePose(new FramePose(ReferenceFrame.getWorldFrame(), position, orientation)); return footstep; }
private Footstep createFootstepAtCurrentLocation(RobotSide robotSide) { ContactablePlaneBody foot = feet.get(robotSide); ReferenceFrame footReferenceFrame = foot.getRigidBody().getParentJoint().getFrameAfterJoint(); FramePose framePose = new FramePose(footReferenceFrame); framePose.changeFrame(worldFrame); PoseReferenceFrame poseReferenceFrame = new PoseReferenceFrame("poseReferenceFrame", framePose); boolean trustHeight = true; Footstep footstep = new Footstep(foot.getRigidBody(), robotSide, foot.getSoleFrame(), poseReferenceFrame, trustHeight); return footstep; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testCopy() { Footstep footstep = new Footstep(RobotSide.RIGHT); FootstepTiming timing = new FootstepTiming(0.1, 1.2); FootstepData obj1 = new FootstepData(footstep, timing); FootstepData obj2 = new FootstepData(); assertTrue(obj2.getFootstep() == null); obj2.set(obj1); assertTrue(obj2.getFootstep() == footstep); assertTrue(obj2.getStepTime() == timing.getStepTime()); } }