diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte index d5a5536..a53d65f 100644 --- a/app/src/lib/components/Visualization.svelte +++ b/app/src/lib/components/Visualization.svelte @@ -74,13 +74,14 @@ let lastTick = performance.now() const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1] + const THREEJS_SCALE = 10 let body_state = { omega: 0, phi: 0, psi: 0, xm: 0, - ym: 0.5, + ym: 0.15, zm: 0, feet: kinematic.getDefaultFeetPos(), cumulative_x: 0, @@ -104,7 +105,7 @@ phi: 0, psi: 0, xm: 0, - ym: 0.7, + ym: 0.15, zm: 0, Background: defaultColor } @@ -239,8 +240,16 @@ const rotatedXm = settings.xm * cosYaw - settings.zm * sinYaw const rotatedZm = settings.xm * sinYaw + settings.zm * cosYaw - robot.position.x = smooth(robot.position.x, -rotatedZm - body_state.cumulative_z * 1.2, 0.1) - robot.position.z = smooth(robot.position.z, -rotatedXm - body_state.cumulative_x * 1.2, 0.1) + robot.position.x = smooth( + robot.position.x, + (-rotatedZm - body_state.cumulative_z) * THREEJS_SCALE, + 0.1 + ) + robot.position.z = smooth( + robot.position.z, + (-rotatedXm - body_state.cumulative_x) * THREEJS_SCALE, + 0.1 + ) const pitch = degToRad(settings.psi - 90) + body_state.cumulative_pitch const roll = degToRad(settings.omega) + body_state.cumulative_roll @@ -275,7 +284,6 @@ s: controlData[5], s1: controlData[6] } - body_state.ym = data.h let planner = planners[get(mode)] const delta = performance.now() - lastTick @@ -296,8 +304,8 @@ settings.omega = radToDeg(robot.rotation.y) settings.phi = radToDeg(robot.rotation.z) + $mpu.heading - 90 settings.psi = radToDeg(robot.rotation.x) + 90 - settings.xm = robot.position.z * 100 - settings.zm = -robot.position.x * 100 + settings.xm = robot.position.z / THREEJS_SCALE + settings.zm = -robot.position.x / THREEJS_SCALE } const updateTargetPosition = () => { diff --git a/app/src/lib/gait.ts b/app/src/lib/gait.ts index e760407..c6d0fe4 100644 --- a/app/src/lib/gait.ts +++ b/app/src/lib/gait.ts @@ -26,21 +26,34 @@ export abstract class GaitState { protected dt = 0.02 protected body_state!: body_state_t + + protected get kinematic() { + return get(currentKinematic) + } + protected gait_state: gait_state_t = { - step_height: 0.4, + step_height: 0, step_x: 0, step_z: 0, step_angle: 0, step_velocity: 1, - step_depth: 0.002 + step_depth: 0 } public get default_feet_pos() { - return get(currentKinematic).getDefaultFeetPos() + return this.kinematic.getDefaultFeetPos() } protected get default_height() { - return 0.5 + return this.kinematic.default_body_height + } + + protected get default_step_depth() { + return this.kinematic.default_step_depth + } + + protected get default_step_height() { + return this.kinematic.default_step_height } begin() { @@ -67,16 +80,15 @@ export abstract class GaitState { } map_command(command: ControllerCommand) { - const newCommand = { - step_height: command.s1, - step_x: command.ly, - step_z: -command.lx, + const kin = this.kinematic + this.gait_state = { + step_height: command.s1 * kin.max_step_height, + step_x: command.ly * kin.max_step_length, + step_z: -command.lx * kin.max_step_length, step_velocity: command.s, step_angle: command.rx, - step_depth: 0.002 + step_depth: kin.default_step_depth } - - this.gait_state = newCommand } } @@ -92,14 +104,13 @@ export class IdleState extends GaitState { export class CalibrationState extends GaitState { protected name = 'Calibration' - // eslint-disable-next-line @typescript-eslint/no-unused-vars step(body_state: body_state_t, _command: ControllerCommand) { super.step(body_state, _command) body_state.omega = 0 body_state.phi = 0 body_state.psi = 0 body_state.xm = 0 - body_state.ym = this.default_height * 10 + body_state.ym = this.kinematic.max_body_height body_state.zm = 0 body_state.feet = this.default_feet_pos return body_state @@ -109,14 +120,13 @@ export class CalibrationState extends GaitState { export class RestState extends GaitState { protected name = 'Rest' - // eslint-disable-next-line @typescript-eslint/no-unused-vars step(body_state: body_state_t, _command: ControllerCommand) { super.step(body_state, _command) body_state.omega = 0 body_state.phi = 0 body_state.psi = 0 body_state.xm = 0 - body_state.ym = this.default_height / 2 + body_state.ym = this.kinematic.min_body_height body_state.zm = 0 body_state.feet = this.default_feet_pos return body_state @@ -128,11 +138,13 @@ export class StandState extends GaitState { step(body_state: body_state_t, command: ControllerCommand) { super.step(body_state, command) + const kin = this.kinematic body_state.omega = 0 - body_state.phi = command.rx * 10 * (Math.PI / 2) - body_state.psi = command.ry * 10 * (Math.PI / 2) - body_state.xm = command.ly / 4 - body_state.zm = command.lx / 4 + body_state.ym = kin.min_body_height + command.h * kin.body_height_range + body_state.psi = command.ry * kin.max_pitch + body_state.phi = command.rx * kin.max_roll + body_state.xm = command.ly * kin.max_body_shift_x + body_state.zm = command.lx * kin.max_body_shift_z body_state.feet = this.default_feet_pos return body_state } @@ -191,6 +203,8 @@ export class BezierState extends GaitState { step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { super.step(body_state, command, dt) + const kin = this.kinematic + this.body_state.ym = kin.min_body_height + command.h * kin.body_height_range this.step_length = Math.sqrt(this.gait_state.step_x ** 2 + this.gait_state.step_z ** 2) if (this.gait_state.step_x < 0) this.step_length = -this.step_length this.update_phase() @@ -339,7 +353,8 @@ export class BezierState extends GaitState { let angle = Math.atan2(this.gait_state.step_z, this.step_length) * 2 const delta_pos = controller(length, angle, ...args, phase) - length = this.gait_state.step_angle * 2 + const kin = this.kinematic + length = this.gait_state.step_angle * kin.max_step_length angle = yawArc(this.default_feet_pos[index], this.body_state.feet[index]) const delta_rot = controller(length, angle, ...args, phase) diff --git a/app/src/lib/kinematic.ts b/app/src/lib/kinematic.ts index 7e4ad1f..e1afa07 100644 --- a/app/src/lib/kinematic.ts +++ b/app/src/lib/kinematic.ts @@ -50,7 +50,22 @@ export default class Kinematic { DEG2RAD = DEG2RAD + max_roll: number + max_pitch: number + max_body_shift_x: number + max_body_shift_z: number + max_leg_reach: number + min_body_height: number + max_body_height: number + body_height_range: number + max_step_length: number + max_step_height: number + default_step_depth: number + default_body_height: number + default_step_height: number + mountOffsets: number[][] + default_feet_positions: number[][] invMountRot = [ [0, 0, -1], @@ -66,18 +81,34 @@ export default class Kinematic { this.L = params.L this.W = params.W + this.max_roll = 15 * (Math.PI / 2) + this.max_pitch = 15 * (Math.PI / 2) + this.max_body_shift_x = this.W / 3 + this.max_body_shift_z = this.W / 3 + this.max_leg_reach = this.femur + this.tibia - this.coxa_offset + this.min_body_height = this.max_leg_reach * 0.45 + this.max_body_height = this.max_leg_reach * 1 + this.body_height_range = this.max_body_height - this.min_body_height + this.max_step_length = this.max_leg_reach * 0.8 + this.max_step_height = this.max_leg_reach / 2 + this.default_step_depth = 0.002 + this.default_body_height = this.min_body_height + this.body_height_range / 2 + this.default_step_height = this.default_body_height / 2 + this.mountOffsets = [ [this.L / 2, 0, this.W / 2], [this.L / 2, 0, -this.W / 2], [-this.L / 2, 0, this.W / 2], [-this.L / 2, 0, -this.W / 2] ] + + this.default_feet_positions = this.mountOffsets.map((offset, i) => { + return [offset[0], 0, offset[2] + (i % 2 === 1 ? -this.coxa : this.coxa)] + }) } getDefaultFeetPos(): number[][] { - return this.mountOffsets.map((offset, i) => { - return [offset[0], -1, offset[2] + (i % 2 === 1 ? -this.coxa : this.coxa)] - }) + return this.default_feet_positions.map(pos => [...pos]) } calcIK(p: body_state_t): number[] { diff --git a/app/src/lib/stores/featureFlags.ts b/app/src/lib/stores/featureFlags.ts index 35a28e0..bf9c291 100644 --- a/app/src/lib/stores/featureFlags.ts +++ b/app/src/lib/stores/featureFlags.ts @@ -29,24 +29,24 @@ export const variants = { model: `${base}spot_micro.urdf.xacro`, stl: `${base}stl.zip`, kinematics: { - coxa: 60.5 / 100, - coxa_offset: 10 / 100, - femur: 111.7 / 100, - tibia: 118.5 / 100, - L: 207.5 / 100, - W: 78 / 100 + coxa: 0.0605, + coxa_offset: 0.01, + femur: 0.1112, + tibia: 0.1185, + L: 0.2075, + W: 0.078 } }, SPOTMICRO_YERTLE: { model: `${base}yertle.URDF`, stl: `${base}URDF.zip`, kinematics: { - coxa: 35 / 100, - coxa_offset: 0 / 100, - femur: 130 / 100, - tibia: 130 / 100, - L: 240 / 100, - W: 78 / 100 + coxa: 0.035, + coxa_offset: 0.0, + femur: 0.13, + tibia: 0.13, + L: 0.24, + W: 0.078 } } } diff --git a/esp32/include/kinematics.h b/esp32/include/kinematics.h index 865dfbd..06cd13a 100644 --- a/esp32/include/kinematics.h +++ b/esp32/include/kinematics.h @@ -6,26 +6,26 @@ class KinConfig { public: #if defined(SPOTMICRO_ESP32) - static constexpr float coxa = 60.5f / 100.0f; - static constexpr float coxa_offset = 10.0f / 100.0f; - static constexpr float femur = 111.2f / 100.0f; - static constexpr float tibia = 118.5f / 100.0f; - static constexpr float L = 207.5f / 100.0f; - static constexpr float W = 78.0f / 100.0f; + static constexpr float coxa = 0.0605f; + static constexpr float coxa_offset = 0.010f; + static constexpr float femur = 0.1112f; + static constexpr float tibia = 0.1185f; + static constexpr float L = 0.2075f; + static constexpr float W = 0.078f; #elif defined(SPOTMICRO_ESP32_MINI) - static constexpr float coxa = 35.0f / 100.0f; - static constexpr float coxa_offset = 0.0f / 100.0f; - static constexpr float femur = 60.0f / 100.0f; - static constexpr float tibia = 60.0f / 100.0f; - static constexpr float L = 160.0f / 100.0f; - static constexpr float W = 80.0f / 100.0f; -#elif defined(SPOTMICRO_YERTLE) - static constexpr float coxa = 35.0f / 100.0f; + static constexpr float coxa = 0.035f; static constexpr float coxa_offset = 0.0f; - static constexpr float femur = 130.0f / 100.0f; - static constexpr float tibia = 130.0f / 100.0f; - static constexpr float L = 240.0f / 100.0f; - static constexpr float W = 78.0f / 100.0f; + static constexpr float femur = 0.060f; + static constexpr float tibia = 0.060f; + static constexpr float L = 0.160f; + static constexpr float W = 0.080f; +#elif defined(SPOTMICRO_YERTLE) + static constexpr float coxa = 0.035f; + static constexpr float coxa_offset = 0.0f; + static constexpr float femur = 0.130f; + static constexpr float tibia = 0.130f; + static constexpr float L = 0.240f; + static constexpr float W = 0.078f; #endif static constexpr float mountOffsets[4][3] = {