323 lines
11 KiB
Python
323 lines
11 KiB
Python
|
import argparse
|
||
|
|
import time
|
||
|
|
import warnings
|
||
|
|
|
||
|
|
import numpy as np
|
||
|
|
import placo
|
||
|
|
from placo_utils.visualization import (
|
||
|
|
footsteps_viz,
|
||
|
|
frame_viz,
|
||
|
|
line_viz,
|
||
|
|
robot_frame_viz,
|
||
|
|
robot_viz,
|
||
|
|
)
|
||
|
|
|
||
|
|
from mini_bdx.bdx_mujoco_server import BDXMujocoServer
|
||
|
|
from mini_bdx.hwi import HWI
|
||
|
|
|
||
|
|
warnings.filterwarnings("ignore")
|
||
|
|
|
||
|
|
parser = argparse.ArgumentParser(description="Process some integers.")
|
||
|
|
parser.add_argument(
|
||
|
|
"-p", "--pybullet", action="store_true", help="PyBullet visualization"
|
||
|
|
)
|
||
|
|
parser.add_argument(
|
||
|
|
"-m", "--meshcat", action="store_true", help="MeshCat visualization"
|
||
|
|
)
|
||
|
|
parser.add_argument("-r", "--robot", action="store_true", help="run on real robot")
|
||
|
|
parser.add_argument("--mujoco", action="store_true", help="Mujoco visualization")
|
||
|
|
args = parser.parse_args()
|
||
|
|
|
||
|
|
DT = 0.01
|
||
|
|
REFINE = 10
|
||
|
|
model_filename = "../../mini_bdx/robots/bdx/robot.urdf"
|
||
|
|
|
||
|
|
# Loading the robot
|
||
|
|
robot = placo.HumanoidRobot(model_filename)
|
||
|
|
robot.set_joint_limits("left_knee", -2, -0.01)
|
||
|
|
robot.set_joint_limits("right_knee", -2, -0.01)
|
||
|
|
|
||
|
|
# Walk parameters - if double_support_ratio is not set to 0, should be greater than replan_frequency
|
||
|
|
parameters = placo.HumanoidParameters()
|
||
|
|
|
||
|
|
parameters.double_support_ratio = 0.2 # Ratio of double support (0.0 to 1.0)
|
||
|
|
parameters.startend_double_support_ratio = (
|
||
|
|
1.5 # Ratio duration of supports for starting and stopping walk
|
||
|
|
)
|
||
|
|
parameters.planned_timesteps = 48 # Number of timesteps planned ahead
|
||
|
|
parameters.replan_timesteps = 10 # Replanning each n timesteps
|
||
|
|
# parameters.zmp_reference_weight = 1e-6
|
||
|
|
|
||
|
|
# Posture parameters
|
||
|
|
parameters.walk_com_height = 0.15 # Constant height for the CoM [m]
|
||
|
|
parameters.walk_foot_height = 0.006 # Height of foot rising while walking [m]
|
||
|
|
parameters.walk_trunk_pitch = np.deg2rad(10) # Trunk pitch angle [rad]
|
||
|
|
parameters.walk_foot_rise_ratio = (
|
||
|
|
0.2 # Time ratio for the foot swing plateau (0.0 to 1.0)
|
||
|
|
)
|
||
|
|
|
||
|
|
# Timing parameters
|
||
|
|
# parameters.single_support_duration = 1 / (
|
||
|
|
# 0.5 * np.sqrt(9.81 / parameters.walk_com_height)
|
||
|
|
# ) # Constant height for the CoM [m] # Duration of single support phase [s]
|
||
|
|
parameters.single_support_duration = 0.2 # Duration of single support phase [s]
|
||
|
|
parameters.single_support_timesteps = (
|
||
|
|
10 # Number of planning timesteps per single support phase
|
||
|
|
)
|
||
|
|
|
||
|
|
# Feet parameters
|
||
|
|
parameters.foot_length = 0.06 # Foot length [m]
|
||
|
|
parameters.foot_width = 0.006 # Foot width [m]
|
||
|
|
parameters.feet_spacing = 0.12 # Lateral feet spacing [m]
|
||
|
|
parameters.zmp_margin = 0.0 # ZMP margin [m]
|
||
|
|
parameters.foot_zmp_target_x = 0.0 # Reference target ZMP position in the foot [m]
|
||
|
|
parameters.foot_zmp_target_y = 0.0 # Reference target ZMP position in the foot [m]
|
||
|
|
|
||
|
|
# Limit parameters
|
||
|
|
parameters.walk_max_dtheta = 1 # Maximum dtheta per step [rad]
|
||
|
|
parameters.walk_max_dy = 0.1 # Maximum dy per step [m]
|
||
|
|
parameters.walk_max_dx_forward = 0.08 # Maximum dx per step forward [m]
|
||
|
|
parameters.walk_max_dx_backward = 0.03 # Maximum dx per step backward [m]
|
||
|
|
|
||
|
|
# Creating the kinematics solver
|
||
|
|
solver = placo.KinematicsSolver(robot)
|
||
|
|
solver.enable_velocity_limits(True)
|
||
|
|
# solver.enable_joint_limits(False)
|
||
|
|
robot.set_velocity_limits(12.0)
|
||
|
|
solver.dt = DT / REFINE
|
||
|
|
|
||
|
|
# Creating the walk QP tasks
|
||
|
|
tasks = placo.WalkTasks()
|
||
|
|
# tasks.trunk_mode = True
|
||
|
|
# tasks.com_x = 0.04
|
||
|
|
tasks.initialize_tasks(solver, robot)
|
||
|
|
tasks.left_foot_task.orientation().mask.set_axises("yz", "local")
|
||
|
|
tasks.right_foot_task.orientation().mask.set_axises("yz", "local")
|
||
|
|
# tasks.trunk_orientation_task.configure("trunk_orientation", "soft", 1e-4)
|
||
|
|
# tasks.left_foot_task.orientation().configure("left_foot_orientation", "soft", 1e-6)
|
||
|
|
# tasks.right_foot_task.orientation().configure("right_foot_orientation", "soft", 1e-6)
|
||
|
|
|
||
|
|
# Creating a joint task to assign DoF values for upper body
|
||
|
|
elbow = -50 * np.pi / 180
|
||
|
|
shoulder_roll = 0 * np.pi / 180
|
||
|
|
shoulder_pitch = 20 * np.pi / 180
|
||
|
|
joints_task = solver.add_joints_task()
|
||
|
|
joints_task.set_joints(
|
||
|
|
{
|
||
|
|
# "left_shoulder_roll": shoulder_roll,
|
||
|
|
# "left_shoulder_pitch": shoulder_pitch,
|
||
|
|
# "left_elbow": elbow,
|
||
|
|
# "right_shoulder_roll": -shoulder_roll,
|
||
|
|
# "right_shoulder_pitch": shoulder_pitch,
|
||
|
|
# "right_elbow": elbow,
|
||
|
|
"head_pitch": 0.0,
|
||
|
|
"head_yaw": 0.0,
|
||
|
|
"neck_pitch": 0.0,
|
||
|
|
"left_antenna": 0.0,
|
||
|
|
"right_antenna": 0.0,
|
||
|
|
# "left_ankle_roll": 0.0,
|
||
|
|
# "right_ankle_roll": 0.0
|
||
|
|
}
|
||
|
|
)
|
||
|
|
joints_task.configure("joints", "soft", 1.0)
|
||
|
|
|
||
|
|
# cam = solver.add_centroidal_momentum_task(np.array([0., 0., 0.]))
|
||
|
|
# cam.mask.set_axises("x", "custom")
|
||
|
|
# cam.mask.R_custom_world = robot.get_T_world_frame("trunk")[:3, :3].T
|
||
|
|
# cam.configure("cam", "soft", 1e-3)
|
||
|
|
|
||
|
|
# Placing the robot in the initial position
|
||
|
|
print("Placing the robot in the initial position...")
|
||
|
|
tasks.reach_initial_pose(
|
||
|
|
np.eye(4),
|
||
|
|
parameters.feet_spacing,
|
||
|
|
parameters.walk_com_height,
|
||
|
|
parameters.walk_trunk_pitch,
|
||
|
|
)
|
||
|
|
print("Initial position reached")
|
||
|
|
|
||
|
|
|
||
|
|
# Creating the FootstepsPlanner
|
||
|
|
repetitive_footsteps_planner = placo.FootstepsPlannerRepetitive(parameters)
|
||
|
|
d_x = 0.1
|
||
|
|
d_y = 0.0
|
||
|
|
d_theta = 0.0
|
||
|
|
nb_steps = 5
|
||
|
|
repetitive_footsteps_planner.configure(d_x, d_y, d_theta, nb_steps)
|
||
|
|
|
||
|
|
# Planning footsteps
|
||
|
|
T_world_left = placo.flatten_on_floor(robot.get_T_world_left())
|
||
|
|
T_world_right = placo.flatten_on_floor(robot.get_T_world_right())
|
||
|
|
footsteps = repetitive_footsteps_planner.plan(
|
||
|
|
placo.HumanoidRobot_Side.left, T_world_left, T_world_right
|
||
|
|
)
|
||
|
|
|
||
|
|
supports = placo.FootstepsPlanner.make_supports(
|
||
|
|
footsteps, True, parameters.has_double_support(), True
|
||
|
|
)
|
||
|
|
|
||
|
|
# Creating the pattern generator and making an initial plan
|
||
|
|
walk = placo.WalkPatternGenerator(robot, parameters)
|
||
|
|
trajectory = walk.plan(supports, robot.com_world(), 0.0)
|
||
|
|
|
||
|
|
if args.pybullet:
|
||
|
|
# Loading the PyBullet simulation
|
||
|
|
import pybullet as p
|
||
|
|
from onshape_to_robot.simulation import Simulation
|
||
|
|
|
||
|
|
sim = Simulation(model_filename, realTime=True, dt=DT, ignore_self_collisions=True)
|
||
|
|
elif args.meshcat:
|
||
|
|
# Starting Meshcat viewer
|
||
|
|
viz = robot_viz(robot)
|
||
|
|
footsteps_viz(trajectory.get_supports())
|
||
|
|
elif args.robot:
|
||
|
|
hwi = HWI()
|
||
|
|
hwi.turn_on()
|
||
|
|
time.sleep(2)
|
||
|
|
elif args.mujoco:
|
||
|
|
time_since_last_right_contact = 0.0
|
||
|
|
time_since_last_left_contact = 0.0
|
||
|
|
bdx_mujoco_server = BDXMujocoServer(
|
||
|
|
model_path="../../mini_bdx/robots/bdx", gravity_on=True
|
||
|
|
)
|
||
|
|
bdx_mujoco_server.start()
|
||
|
|
else:
|
||
|
|
print("No visualization selected, use either -p or -m")
|
||
|
|
exit()
|
||
|
|
|
||
|
|
# Timestamps
|
||
|
|
start_t = time.time()
|
||
|
|
initial_delay = -1.0
|
||
|
|
t = initial_delay
|
||
|
|
last_display = time.time()
|
||
|
|
last_replan = 0
|
||
|
|
petage_de_gueule = False
|
||
|
|
got_input = False
|
||
|
|
while True:
|
||
|
|
if got_input:
|
||
|
|
repetitive_footsteps_planner.configure(d_x, d_y, d_theta, nb_steps)
|
||
|
|
got_input = False
|
||
|
|
|
||
|
|
# Invoking the IK QP solver
|
||
|
|
for k in range(REFINE):
|
||
|
|
# Updating the QP tasks from planned trajectory
|
||
|
|
if not petage_de_gueule:
|
||
|
|
tasks.update_tasks_from_trajectory(trajectory, t - DT + k * DT / REFINE)
|
||
|
|
|
||
|
|
robot.update_kinematics()
|
||
|
|
qd_sol = solver.solve(True)
|
||
|
|
# solver.dump_status()
|
||
|
|
|
||
|
|
# Ensuring the robot is kinematically placed on the floor on the proper foot to avoid integration drifts
|
||
|
|
# if not trajectory.support_is_both(t):
|
||
|
|
# robot.update_support_side(str(trajectory.support_side(t)))
|
||
|
|
# robot.ensure_on_floor()
|
||
|
|
|
||
|
|
# If enough time elapsed and we can replan, do the replanning
|
||
|
|
if (
|
||
|
|
t - last_replan > parameters.replan_timesteps * parameters.dt()
|
||
|
|
and walk.can_replan_supports(trajectory, t)
|
||
|
|
):
|
||
|
|
last_replan = t
|
||
|
|
|
||
|
|
# Replanning footsteps from current trajectory
|
||
|
|
supports = walk.replan_supports(repetitive_footsteps_planner, trajectory, t)
|
||
|
|
|
||
|
|
# Replanning CoM trajectory, yielding a new trajectory we can switch to
|
||
|
|
trajectory = walk.replan(supports, trajectory, t)
|
||
|
|
|
||
|
|
if args.meshcat:
|
||
|
|
# Drawing footsteps
|
||
|
|
footsteps_viz(supports)
|
||
|
|
|
||
|
|
# Drawing planned CoM trajectory on the ground
|
||
|
|
coms = [
|
||
|
|
[*trajectory.get_p_world_CoM(t)[:2], 0.0]
|
||
|
|
for t in np.linspace(trajectory.t_start, trajectory.t_end, 100)
|
||
|
|
]
|
||
|
|
line_viz("CoM_trajectory", np.array(coms), 0xFFAA00)
|
||
|
|
|
||
|
|
# During the warmup phase, the robot is enforced to stay in the initial position
|
||
|
|
if args.pybullet:
|
||
|
|
if t < -2:
|
||
|
|
T_left_origin = sim.transformation("origin", "left_foot_frame")
|
||
|
|
T_world_left = sim.poseToMatrix(([0.0, 0.0, 0.05], [0.0, 0.0, 0.0, 1.0]))
|
||
|
|
T_world_origin = T_world_left @ T_left_origin
|
||
|
|
|
||
|
|
sim.setRobotPose(*sim.matrixToPose(T_world_origin))
|
||
|
|
|
||
|
|
joints = {joint: robot.get_joint(joint) for joint in sim.getJoints()}
|
||
|
|
applied = sim.setJoints(joints)
|
||
|
|
sim.tick()
|
||
|
|
|
||
|
|
# Updating meshcat display periodically
|
||
|
|
elif args.meshcat:
|
||
|
|
if time.time() - last_display > 0.01:
|
||
|
|
last_display = time.time()
|
||
|
|
viz.display(robot.state.q)
|
||
|
|
|
||
|
|
# frame_viz("left_foot_target", trajectory.get_T_world_left(t))
|
||
|
|
# frame_viz("right_foot_target", trajectory.get_T_world_right(t))
|
||
|
|
# robot_frame_viz(robot, "left_foot")
|
||
|
|
# robot_frame_viz(robot, "right_foot")
|
||
|
|
|
||
|
|
T_world_trunk = np.eye(4)
|
||
|
|
T_world_trunk[:3, :3] = trajectory.get_R_world_trunk(t)
|
||
|
|
T_world_trunk[:3, 3] = trajectory.get_p_world_CoM(t)
|
||
|
|
frame_viz("trunk_target", T_world_trunk)
|
||
|
|
# footsteps_viz(trajectory.get_supports())
|
||
|
|
|
||
|
|
if args.robot or args.mujoco:
|
||
|
|
angles = {
|
||
|
|
"right_hip_yaw": robot.get_joint("right_hip_yaw"),
|
||
|
|
"right_hip_roll": robot.get_joint("right_hip_roll"),
|
||
|
|
"right_hip_pitch": robot.get_joint("right_hip_pitch"),
|
||
|
|
"right_knee": robot.get_joint("right_knee"),
|
||
|
|
"right_ankle": robot.get_joint("right_ankle"),
|
||
|
|
"left_hip_yaw": robot.get_joint("left_hip_yaw"),
|
||
|
|
"left_hip_roll": robot.get_joint("left_hip_roll"),
|
||
|
|
"left_hip_pitch": robot.get_joint("left_hip_pitch"),
|
||
|
|
"left_knee": robot.get_joint("left_knee"),
|
||
|
|
"left_ankle": robot.get_joint("left_ankle"),
|
||
|
|
"neck_pitch": robot.get_joint("neck_pitch"),
|
||
|
|
"head_pitch": robot.get_joint("head_pitch"),
|
||
|
|
"head_yaw": robot.get_joint("head_yaw"),
|
||
|
|
}
|
||
|
|
if args.robot:
|
||
|
|
hwi.set_position_all(angles)
|
||
|
|
elif args.mujoco:
|
||
|
|
right_contact, left_contact = bdx_mujoco_server.get_feet_contact()
|
||
|
|
if left_contact:
|
||
|
|
time_since_last_left_contact = 0.0
|
||
|
|
if right_contact:
|
||
|
|
time_since_last_right_contact = 0.0
|
||
|
|
# print("time since last left contact :", time_since_last_left_contact)
|
||
|
|
# print("time since last right contact :", time_since_last_right_contact)
|
||
|
|
bdx_mujoco_server.send_action(list(angles.values()))
|
||
|
|
|
||
|
|
if (
|
||
|
|
time_since_last_left_contact > parameters.single_support_duration
|
||
|
|
or time_since_last_right_contact > parameters.single_support_duration
|
||
|
|
):
|
||
|
|
petage_de_gueule = True
|
||
|
|
# print("pétage de gueule")
|
||
|
|
else:
|
||
|
|
petage_de_gueule = False
|
||
|
|
|
||
|
|
time_since_last_left_contact += DT
|
||
|
|
time_since_last_right_contact += DT
|
||
|
|
|
||
|
|
if bdx_mujoco_server.key_pressed is not None:
|
||
|
|
got_input = True
|
||
|
|
d_x = 0.05
|
||
|
|
else:
|
||
|
|
got_input = True
|
||
|
|
d_x = 0
|
||
|
|
d_y = 0
|
||
|
|
d_theta = 0
|
||
|
|
|
||
|
|
t += DT
|
||
|
|
# print(t)
|
||
|
|
while time.time() < start_t + t:
|
||
|
|
time.sleep(1e-5)
|