private double getPelvisToFoot(HumanoidFloatingRootJointRobot robot) { List<GroundContactPoint> contactPoints = robot.getFootGroundContactPoints(RobotSide.LEFT); double height = Double.POSITIVE_INFINITY; for(GroundContactPoint gc : contactPoints) { if(gc.getPositionPoint().getZ() < height) { height = gc.getPositionPoint().getZ(); } } return offset.getZ() - height; }
private double getPelvisToFoot(HumanoidFloatingRootJointRobot robot) { List<GroundContactPoint> contactPoints = robot.getFootGroundContactPoints(RobotSide.LEFT); double height = Double.POSITIVE_INFINITY; if (contactPoints.size() == 0) height = -1.0050100629487357; else { for (GroundContactPoint gc : contactPoints) { if (gc.getPositionPoint().getZ() < height) { height = gc.getPositionPoint().getZ(); } } } return offset.getZ() - height; }
protected void setPositionInWorld(HumanoidFloatingRootJointRobot robot) { robot.update(); robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.getTranslation(offset); Vector3d positionInWorld = new Vector3d(); rootToWorld.getTranslation(positionInWorld); GroundContactPoint gc1 = robot.getFootGroundContactPoints(RobotSide.LEFT).get(0); double pelvisToFoot = positionInWorld.getZ() - gc1.getPositionPoint().getZ(); offset.setZ(groundZ + pelvisToFoot); robot.setPositionInWorld(offset); }
protected void setPositionInWorld(HumanoidFloatingRootJointRobot robot) { robot.update(); robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.getTranslation(offset); Vector3D positionInWorld = new Vector3D(); rootToWorld.getTranslation(positionInWorld); GroundContactPoint gc1 = robot.getFootGroundContactPoints(RobotSide.LEFT).get(0); double pelvisToFoot = positionInWorld.getZ() - gc1.getPositionPoint().getZ(); offset.setZ(groundZ + pelvisToFoot); robot.setPositionInWorld(offset); }
protected void setPositionInWorld(HumanoidFloatingRootJointRobot robot) { robot.update(); robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.getTranslation(offset); Vector3d positionInWorld = new Vector3d(); rootToWorld.getTranslation(positionInWorld); GroundContactPoint gc1 = robot.getFootGroundContactPoints(RobotSide.LEFT).get(0); double pelvisToFoot = positionInWorld.getZ() - gc1.getPositionPoint().getZ(); offset.setZ(groundZ + pelvisToFoot); robot.setPositionInWorld(offset); }
public void initializeRobot(HumanoidFloatingRootJointRobot robot, DRCRobotJointMap jointMap) { for(RobotSide robotSide : RobotSide.values()) { String prefix = robotSide.getSideNameFirstLetter().toLowerCase(); robot.getOneDegreeOfFreedomJoint(prefix + "_leg_lhy").setQ(-0.4); robot.getOneDegreeOfFreedomJoint(prefix + "_leg_kny").setQ(0.8); robot.getOneDegreeOfFreedomJoint(prefix + "_leg_uay").setQ((-0.4)); } robot.update(); robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.get(rotation, offset); GroundContactPoint gc1 = robot.getFootGroundContactPoints(RobotSide.LEFT).get(0); double pelvisToFoot = offset.getZ() - gc1.getPositionPoint().getZ(); // Hardcoded for gazebo integration // double pelvisToFoot = 0.887; offset.setZ(groundZ + pelvisToFoot); // offset.add(robot.getPositionInWorld()); robot.setPositionInWorld(offset); FrameQuaternion frameOrientation = new FrameQuaternion(ReferenceFrame.getWorldFrame(), rotation); double[] yawPitchRoll = new double[3]; frameOrientation.getYawPitchRoll(yawPitchRoll); yawPitchRoll[0] = initialYaw; frameOrientation.setYawPitchRoll(yawPitchRoll); robot.setOrientation(frameOrientation); robot.update(); }
public void initializeRobot(HumanoidFloatingRootJointRobot robot, DRCRobotJointMap jointMap) { for(RobotSide robotSide : RobotSide.values()) { String prefix = robotSide.getSideNameFirstLetter().toLowerCase(); robot.getOneDegreeOfFreedomJoint(prefix + "_leg_lhy").setQ(-0.4); robot.getOneDegreeOfFreedomJoint(prefix + "_leg_kny").setQ(0.8); robot.getOneDegreeOfFreedomJoint(prefix + "_leg_uay").setQ((-0.4)); } robot.update(); robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.get(rotation, offset); GroundContactPoint gc1 = robot.getFootGroundContactPoints(RobotSide.LEFT).get(0); double pelvisToFoot = offset.getZ() - gc1.getPositionPoint().getZ(); // Hardcoded for gazebo integration // double pelvisToFoot = 0.887; offset.setZ(groundZ + pelvisToFoot); // offset.add(robot.getPositionInWorld()); robot.setPositionInWorld(offset); FrameQuaternion frameOrientation = new FrameQuaternion(ReferenceFrame.getWorldFrame(), rotation); double[] yawPitchRoll = new double[3]; frameOrientation.getYawPitchRoll(yawPitchRoll); yawPitchRoll[0] = initialYaw; frameOrientation.setYawPitchRoll(yawPitchRoll); robot.setOrientation(frameOrientation); robot.update(); }