diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/.gitignore b/Open_Duck_Playground-main/Open_Duck_Playground-main/.gitignore new file mode 100644 index 0000000..211dc36 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/.gitignore @@ -0,0 +1,11 @@ +Open_Duck_Playground.egg-info/* +uv.lock +__pycache__/ +playground/sigmaban2019/ +playground/open_duck_mini_v2_skate/ +robot_saved_obs.pkl +mujoco_saved_obs.pkl +checkpoints/ +.tmp/ +*.onnx +*.TXT \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/README.md b/Open_Duck_Playground-main/Open_Duck_Playground-main/README.md new file mode 100644 index 0000000..cb06d85 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/README.md @@ -0,0 +1,98 @@ +# Open Duck Playground + +# Installation + +Install uv + +```bash +curl -LsSf https://astral.sh/uv/install.sh | sh +``` + +# Training + +If you want to use the [imitation reward](https://la.disneyresearch.com/wp-content/uploads/BD_X_paper.pdf), you can generate reference motion with [this repo](https://github.com/apirrone/Open_Duck_reference_motion_generator) + +Then copy `polynomial_coefficients.pkl` in `playground//data/` + +You'll also have to set `USE_IMITATION_REWARD=True` in it's `joystick.py` file + +Run: + +```bash +uv run playground//runner.py +``` + +## Tensorboard + +```bash +uv run tensorboard --logdir= +``` + +# Inference + +Infer mujoco + +(for now this is specific to open_duck_mini_v2) + +```bash +uv run playground/open_duck_mini_v2/mujoco_infer.py -o +``` + +# Documentation + +## Project structure : + +``` +. +├── pyproject.toml +├── README.md +├── playground +│   ├── common +│   │   ├── export_onnx.py +│   │   ├── onnx_infer.py +│   │   ├── poly_reference_motion.py +│   │   ├── randomize.py +│   │   ├── rewards.py +│   │   └── runner.py +│   ├── open_duck_mini_v2 +│   │   ├── base.py +│   │   ├── data +│   │   │   └── polynomial_coefficients.pkl +│   │   ├── joystick.py +│   │   ├── mujoco_infer.py +│   │   ├── constants.py +│   │   ├── runner.py +│   │   └── xmls +│   │   ├── assets +│   │   ├── open_duck_mini_v2_no_head.xml +│   │   ├── open_duck_mini_v2.xml +│   │   ├── scene_mjx_flat_terrain.xml +│   │   ├── scene_mjx_rough_terrain.xml +│   │   └── scene.xml +``` + +## Adding a new robot + +Create a new directory in `playground` named after ``. You can copy the `open_duck_mini_v2` directory as a starting point. + +You will need to: +- Edit `base.py`: Mainly renaming stuff to match you robot's name +- Edit `constants.py`: specify the names of some important geoms, sensors etc + - In your `mjcf`, you'll probably have to add some sites, name some bodies/geoms and add the sensors. Look at how we did it for `open_duck_mini_v2` +- Add your `mjcf` assets in `xmls`. +- Edit `joystick.py` : to choose the rewards you are interested in + - Note: for now there is still some hard coded values etc. We'll improve things on the way +- Edit `runner.py` + + + +# Notes + +Inspired from https://github.com/kscalelabs/mujoco_playground + + +## Current win + +```bash +uv run playground/open_duck_mini_v2/runner.py --task flat_terrain_backlash --num_timesteps 300000000 +``` diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/aze b/Open_Duck_Playground-main/Open_Duck_Playground-main/aze new file mode 100644 index 0000000..e69de29 diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/__init__.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/__init__.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/export_onnx.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/export_onnx.py new file mode 100644 index 0000000..2c7f427 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/export_onnx.py @@ -0,0 +1,189 @@ + +import tensorflow as tf +from tensorflow.keras import layers +import tf2onnx +import numpy as np + +def export_onnx( + params, act_size, ppo_params, obs_size, output_path="ONNX.onnx" +): + print(" === EXPORT ONNX === ") + + # inference_fn = make_inference_fn(params, deterministic=True) + + class MLP(tf.keras.Model): + def __init__( + self, + layer_sizes, + activation=tf.nn.relu, + kernel_init="lecun_uniform", + activate_final=False, + bias=True, + layer_norm=False, + mean_std=None, + ): + super().__init__() + + self.layer_sizes = layer_sizes + self.activation = activation + self.kernel_init = kernel_init + self.activate_final = activate_final + self.bias = bias + self.layer_norm = layer_norm + + if mean_std is not None: + self.mean = tf.Variable(mean_std[0], trainable=False, dtype=tf.float32) + self.std = tf.Variable(mean_std[1], trainable=False, dtype=tf.float32) + else: + self.mean = None + self.std = None + + self.mlp_block = tf.keras.Sequential(name="MLP_0") + for i, size in enumerate(self.layer_sizes): + dense_layer = layers.Dense( + size, + activation=self.activation, + kernel_initializer=self.kernel_init, + name=f"hidden_{i}", + use_bias=self.bias, + ) + self.mlp_block.add(dense_layer) + if self.layer_norm: + self.mlp_block.add( + layers.LayerNormalization(name=f"layer_norm_{i}") + ) + if not self.activate_final and self.mlp_block.layers: + if ( + hasattr(self.mlp_block.layers[-1], "activation") + and self.mlp_block.layers[-1].activation is not None + ): + self.mlp_block.layers[-1].activation = None + + self.submodules = [self.mlp_block] + + def call(self, inputs): + if isinstance(inputs, list): + inputs = inputs[0] + if self.mean is not None and self.std is not None: + print(self.mean.shape, self.std.shape) + inputs = (inputs - self.mean) / self.std + logits = self.mlp_block(inputs) + loc, _ = tf.split(logits, 2, axis=-1) + return tf.tanh(loc) + + def make_policy_network( + param_size, + mean_std, + hidden_layer_sizes=[256, 256], + activation=tf.nn.relu, + kernel_init="lecun_uniform", + layer_norm=False, + ): + policy_network = MLP( + layer_sizes=list(hidden_layer_sizes) + [param_size], + activation=activation, + kernel_init=kernel_init, + layer_norm=layer_norm, + mean_std=mean_std, + ) + return policy_network + + mean = params[0].mean["state"] + std = params[0].std["state"] + + # Convert mean/std jax arrays to tf tensors. + mean_std = (tf.convert_to_tensor(mean), tf.convert_to_tensor(std)) + + tf_policy_network = make_policy_network( + param_size=act_size * 2, + mean_std=mean_std, + hidden_layer_sizes=ppo_params.network_factory.policy_hidden_layer_sizes, + activation=tf.nn.swish, + ) + + example_input = tf.zeros((1, obs_size)) + example_output = tf_policy_network(example_input) + print(example_output.shape) + + def transfer_weights(jax_params, tf_model): + """ + Transfer weights from a JAX parameter dictionary to the TensorFlow model. + + Parameters: + - jax_params: dict + Nested dictionary with structure {block_name: {layer_name: {params}}}. + For example: + { + 'CNN_0': { + 'Conv_0': {'kernel': np.ndarray}, + 'Conv_1': {'kernel': np.ndarray}, + 'Conv_2': {'kernel': np.ndarray}, + }, + 'MLP_0': { + 'hidden_0': {'kernel': np.ndarray, 'bias': np.ndarray}, + 'hidden_1': {'kernel': np.ndarray, 'bias': np.ndarray}, + 'hidden_2': {'kernel': np.ndarray, 'bias': np.ndarray}, + } + } + + - tf_model: tf.keras.Model + An instance of the adapted VisionMLP model containing named submodules and layers. + """ + for layer_name, layer_params in jax_params.items(): + try: + tf_layer = tf_model.get_layer("MLP_0").get_layer(name=layer_name) + except ValueError: + print(f"Layer {layer_name} not found in TensorFlow model.") + continue + if isinstance(tf_layer, tf.keras.layers.Dense): + kernel = np.array(layer_params["kernel"]) + bias = np.array(layer_params["bias"]) + print( + f"Transferring Dense layer {layer_name}, kernel shape {kernel.shape}, bias shape {bias.shape}" + ) + tf_layer.set_weights([kernel, bias]) + else: + print(f"Unhandled layer type in {layer_name}: {type(tf_layer)}") + + print("Weights transferred successfully.") + + net_params = params[1] + # try attribute‐style .policy + if hasattr(net_params, "policy"): + policy_tree = net_params.policy + # try dict‐style ["policy"] + elif isinstance(net_params, dict) and "policy" in net_params: + policy_tree = net_params["policy"] + # fallback to Flax FrozenDict { "params": … } + elif isinstance(net_params, dict) and "params" in net_params: + policy_tree = net_params["params"] + else: + raise KeyError(f"Cannot locate policy params in {type(net_params)}; keys = {list(net_params.keys())}") + + # policy_tree is now the dict of weight arrays + transfer_weights(policy_tree, tf_policy_network) + + # Example inputs for the model + test_input = [np.ones((1, obs_size), dtype=np.float32)] + + # Define the TensorFlow input signature + spec = [ + tf.TensorSpec(shape=(1, obs_size), dtype=tf.float32, name="obs") + ] + + tensorflow_pred = tf_policy_network(test_input)[0] + # Build the model by calling it with example data + print(f"Tensorflow prediction: {tensorflow_pred}") + + tf_policy_network.output_names = ["continuous_actions"] + + # opset 11 matches isaac lab. + model_proto, _ = tf2onnx.convert.from_keras( + tf_policy_network, input_signature=spec, opset=11, output_path=output_path + ) + + # For Antoine :) + model_proto, _ = tf2onnx.convert.from_keras( + tf_policy_network, input_signature=spec, opset=11, output_path="ONNX.onnx" + ) + return diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/onnx_infer.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/onnx_infer.py new file mode 100644 index 0000000..29c9ad9 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/onnx_infer.py @@ -0,0 +1,46 @@ +import onnxruntime + + +class OnnxInfer: + def __init__(self, onnx_model_path, input_name="obs", awd=False): + self.onnx_model_path = onnx_model_path + self.ort_session = onnxruntime.InferenceSession( + self.onnx_model_path, providers=["CPUExecutionProvider"] + ) + self.input_name = input_name + self.awd = awd + + def infer(self, inputs): + if self.awd: + outputs = self.ort_session.run(None, {self.input_name: [inputs]}) + return outputs[0][0] + else: + outputs = self.ort_session.run( + None, {self.input_name: inputs.astype("float32")} + ) + return outputs[0] + + +if __name__ == "__main__": + import argparse + import numpy as np + import time + + parser = argparse.ArgumentParser() + parser.add_argument("-o", "--onnx_model_path", type=str, required=True) + args = parser.parse_args() + + obs_size = 46 + + oi = OnnxInfer(args.onnx_model_path, awd=True) + times = [] + for i in range(1000): + inputs = np.random.uniform(size=obs_size).astype(np.float32) + # inputs = np.arange(obs_size).astype(np.float32) + # print(inputs) + start = time.time() + print(oi.infer(inputs)) + times.append(time.time() - start) + + print("Average time: ", sum(times) / len(times)) + print("Average fps: ", 1 / (sum(times) / len(times))) diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/plot_saved_obs.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/plot_saved_obs.py new file mode 100644 index 0000000..00bd1c1 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/plot_saved_obs.py @@ -0,0 +1,222 @@ +import pickle + +import matplotlib.pyplot as plt +import numpy as np +from scipy.spatial.transform import Rotation as R +import argparse + +parser = argparse.ArgumentParser() +parser.add_argument( + "-d", "--data", type=str, required=False, default="mujoco_saved_obs.pkl" +) +args = parser.parse_args() + + +init_pos = np.array( + [ + 0.002, + 0.053, + -0.63, + 1.368, + -0.784, + 0.0, + 0, + 0, + 0, + # 0, + # 0, + -0.003, + -0.065, + 0.635, + 1.379, + -0.796, + ] +) + +joints_order = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", +] + +obses = pickle.load(open(args.data, "rb")) + +num_dofs = 14 +dof_poses = [] # (dof, num_obs) +actions = [] # (dof, num_obs) + +for i in range(num_dofs): + print(i) + dof_poses.append([]) + actions.append([]) + for obs in obses: + dof_poses[i].append(obs[13 : 13 + num_dofs][i]) + actions[i].append(obs[26 : 26 + num_dofs][i]) + +# plot action vs dof pos + +nb_dofs = len(dof_poses) +nb_rows = int(np.sqrt(nb_dofs)) +nb_cols = int(np.ceil(nb_dofs / nb_rows)) + +fig, axs = plt.subplots(nb_rows, nb_cols, sharex=True, sharey=True) + +for i in range(nb_rows): + for j in range(nb_cols): + if i * nb_cols + j >= nb_dofs: + break + axs[i, j].plot(actions[i * nb_cols + j], label="action") + axs[i, j].plot(dof_poses[i * nb_cols + j], label="dof_pos") + axs[i, j].legend() + axs[i, j].set_title(f"{joints_order[i * nb_cols + j]}") + +fig.suptitle(f"{args.data}") +plt.show() + +obses_names = [ + "gyro x", + "gyro y", + "gyro z", + "accelo x", + "accelo y", + "accelo z", + # commands + "command 0", + "command 1", + "command 2", + "command 3", + "command 4", + "command 5", + "command 6", + # dof pos + "pos_" + str(joints_order[0]), + "pos_" + str(joints_order[1]), + "pos_" + str(joints_order[2]), + "pos_" + str(joints_order[3]), + "pos_" + str(joints_order[4]), + "pos_" + str(joints_order[5]), + "pos_" + str(joints_order[6]), + "pos_" + str(joints_order[7]), + "pos_" + str(joints_order[8]), + "pos_" + str(joints_order[9]), + "pos_" + str(joints_order[10]), + "pos_" + str(joints_order[11]), + "pos_" + str(joints_order[12]), + "pos_" + str(joints_order[13]), + # dof vel + "vel_" + str(joints_order[0]), + "vel_" + str(joints_order[1]), + "vel_" + str(joints_order[2]), + "vel_" + str(joints_order[3]), + "vel_" + str(joints_order[4]), + "vel_" + str(joints_order[5]), + "vel_" + str(joints_order[6]), + "vel_" + str(joints_order[7]), + "vel_" + str(joints_order[8]), + "vel_" + str(joints_order[9]), + "vel_" + str(joints_order[10]), + "vel_" + str(joints_order[11]), + "vel_" + str(joints_order[12]), + "vel_" + str(joints_order[13]), + # action + "last_action_" + str(joints_order[0]), + "last_action_" + str(joints_order[1]), + "last_action_" + str(joints_order[2]), + "last_action_" + str(joints_order[3]), + "last_action_" + str(joints_order[4]), + "last_action_" + str(joints_order[5]), + "last_action_" + str(joints_order[6]), + "last_action_" + str(joints_order[7]), + "last_action_" + str(joints_order[8]), + "last_action_" + str(joints_order[9]), + "last_action_" + str(joints_order[10]), + "last_action_" + str(joints_order[11]), + "last_action_" + str(joints_order[12]), + "last_action_" + str(joints_order[13]), + "last_last_action_" + str(joints_order[0]), + "last_last_action_" + str(joints_order[1]), + "last_last_action_" + str(joints_order[2]), + "last_last_action_" + str(joints_order[3]), + "last_last_action_" + str(joints_order[4]), + "last_last_action_" + str(joints_order[5]), + "last_last_action_" + str(joints_order[6]), + "last_last_action_" + str(joints_order[7]), + "last_last_action_" + str(joints_order[8]), + "last_last_action_" + str(joints_order[9]), + "last_last_action_" + str(joints_order[10]), + "last_last_action_" + str(joints_order[11]), + "last_last_action_" + str(joints_order[12]), + "last_last_action_" + str(joints_order[13]), + "last_last_last_action_" + str(joints_order[0]), + "last_last_last_action_" + str(joints_order[1]), + "last_last_last_action_" + str(joints_order[2]), + "last_last_last_action_" + str(joints_order[3]), + "last_last_last_action_" + str(joints_order[4]), + "last_last_last_action_" + str(joints_order[5]), + "last_last_last_action_" + str(joints_order[6]), + "last_last_last_action_" + str(joints_order[7]), + "last_last_last_action_" + str(joints_order[8]), + "last_last_last_action_" + str(joints_order[9]), + "last_last_last_action_" + str(joints_order[10]), + "last_last_last_action_" + str(joints_order[11]), + "last_last_last_action_" + str(joints_order[12]), + "last_last_last_action_" + str(joints_order[13]), + "motor_targets_" + str(joints_order[0]), + "motor_targets_" + str(joints_order[1]), + "motor_targets_" + str(joints_order[2]), + "motor_targets_" + str(joints_order[3]), + "motor_targets_" + str(joints_order[4]), + "motor_targets_" + str(joints_order[5]), + "motor_targets_" + str(joints_order[6]), + "motor_targets_" + str(joints_order[7]), + "motor_targets_" + str(joints_order[8]), + "motor_targets_" + str(joints_order[9]), + "motor_targets_" + str(joints_order[10]), + "motor_targets_" + str(joints_order[11]), + "motor_targets_" + str(joints_order[12]), + "motor_targets_" + str(joints_order[13]), + "contact left", + "contact right", + "imitation_phase 1", + "imitation_phase 2" + # ref (ignored) +] +# print(len(obses_names)) +# exit() + + +# obses = [[56 obs at time 0], [56 obs at time 1], ...] + +nb_obs = len(obses[0]) +print(nb_obs) +nb_rows = int(np.sqrt(nb_obs)) +nb_cols = int(np.ceil(nb_obs / nb_rows)) + +fig, axs = plt.subplots(nb_rows, nb_cols, sharex=True, sharey=True) + +for i in range(nb_rows): + for j in range(nb_cols): + if i * nb_cols + j >= nb_obs: + break + axs[i, j].plot([obs[i * nb_cols + j] for obs in obses]) + axs[i, j].set_title(obses_names[i * nb_cols + j]) + +# set ylim between -5 and 5 + +for ax in axs.flat: + ax.set_ylim([-5, 5]) + + +fig.suptitle(f"{args.data}") +plt.show() diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/poly_reference_motion.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/poly_reference_motion.py new file mode 100644 index 0000000..62326af --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/poly_reference_motion.py @@ -0,0 +1,187 @@ +import jax.numpy as jp +from jax import vmap +import pickle + + +# dimensions_names = [ +# 0 "pos left_hip_yaw", +# 1 "pos left_hip_roll", +# 2 "pos left_hip_pitch", +# 3 "pos left_knee", +# 4 "pos left_ankle", +# 5 "pos neck_pitch", +# 6 "pos head_pitch", +# 7 "pos head_yaw", +# 8 "pos head_roll", +# 9 "pos left_antenna", +# 10 "pos right_antenna", +# 11 "pos right_hip_yaw", +# 12 "pos right_hip_roll", +# 13 "pos right_hip_pitch", +# 14 "pos right_knee", +# 15 "pos right_ankle", + +# 16 "vel left_hip_yaw", +# 17 "vel left_hip_roll", +# 18 "vel left_hip_pitch", +# 19 "vel left_knee", +# 20 "vel left_ankle", +# 21 "vel neck_pitch", +# 22 "vel head_pitch", +# 23 "vel head_yaw", +# 24 "vel head_roll", +# 25 "vel left_antenna", +# 26 "vel right_antenna", +# 27 "vel right_hip_yaw", +# 28 "vel right_hip_roll", +# 29 "vel right_hip_pitch", +# 30 "vel right_knee", +# 31 "vel right_ankle", + +# 32 "foot_contacts left", +# 33 "foot_contacts right", + +# 34 "base_linear_vel x", +# 35 "base_linear_vel y", +# 36 "base_linear_vel z", + +# 37 "base_angular_vel x", +# 38 "base_angular_vel y", +# 39 "base_angular_vel z", +# ] + + +class PolyReferenceMotion: + def __init__(self, polynomial_coefficients: str): + data = pickle.load(open(polynomial_coefficients, "rb")) + # data = json.load(open(polynomial_coefficients)) + self.dx_range = [0, 0] + self.dy_range = [0, 0] + self.dtheta_range = [0, 0] + self.dxs = [] + self.dys = [] + self.dthetas = [] + self.data_array = [] + self.period = None + self.fps = None + self.frame_offsets = None + self.startend_double_support_ratio = None + self.start_offset = None + self.nb_steps_in_period = None + + self.process(data) + + def process(self, data): + print("[Poly ref data] Processing ...") + _data = {} + for name in data.keys(): + split = name.split("_") + dx = float(split[0]) + dy = float(split[1]) + dtheta = float(split[2]) + + if self.period is None: + self.period = data[name]["period"] + self.fps = data[name]["fps"] + self.frame_offsets = data[name]["frame_offsets"] + self.startend_double_support_ratio = data[name][ + "startend_double_support_ratio" + ] + self.start_offset = int(self.startend_double_support_ratio * self.fps) + self.nb_steps_in_period = int(self.period * self.fps) + + if dx not in self.dxs: + self.dxs.append(dx) + + if dy not in self.dys: + self.dys.append(dy) + + if dtheta not in self.dthetas: + self.dthetas.append(dtheta) + + self.dx_range = [min(dx, self.dx_range[0]), max(dx, self.dx_range[1])] + self.dy_range = [min(dy, self.dy_range[0]), max(dy, self.dy_range[1])] + self.dtheta_range = [ + min(dtheta, self.dtheta_range[0]), + max(dtheta, self.dtheta_range[1]), + ] + + if dx not in _data: + _data[dx] = {} + + if dy not in _data[dx]: + _data[dx][dy] = {} + + if dtheta not in _data[dx][dy]: + _data[dx][dy][dtheta] = data[name] + + _coeffs = data[name]["coefficients"] + + coeffs = [] + for k, v in _coeffs.items(): + coeffs.append(jp.flip(jp.array(v))) + _data[dx][dy][dtheta] = coeffs + + # print(self.dtheta_range) + # exit() + + self.dxs = sorted(self.dxs) + self.dys = sorted(self.dys) + self.dthetas = sorted(self.dthetas) + + nb_dx = len(self.dxs) + nb_dy = len(self.dys) + nb_dtheta = len(self.dthetas) + + self.data_array = nb_dx * [None] + for x, dx in enumerate(self.dxs): + self.data_array[x] = nb_dy * [None] + for y, dy in enumerate(self.dys): + self.data_array[x][y] = nb_dtheta * [None] + for th, dtheta in enumerate(self.dthetas): + self.data_array[x][y][th] = jp.array(_data[dx][dy][dtheta]) + + self.data_array = jp.array(self.data_array) + + print("[Poly ref data] Done processing") + + def vel_to_index(self, dx, dy, dtheta): + + dx = jp.clip(dx, self.dx_range[0], self.dx_range[1]) + dy = jp.clip(dy, self.dy_range[0], self.dy_range[1]) + dtheta = jp.clip(dtheta, self.dtheta_range[0], self.dtheta_range[1]) + + ix = jp.argmin(jp.abs(jp.array(self.dxs) - dx)) + iy = jp.argmin(jp.abs(jp.array(self.dys) - dy)) + itheta = jp.argmin(jp.abs(jp.array(self.dthetas) - dtheta)) + + return ix, iy, itheta + + def sample_polynomial(self, t, coeffs): + return vmap(lambda c: jp.polyval(c, t))(coeffs) + + def get_reference_motion(self, dx, dy, dtheta, i): + ix, iy, itheta = self.vel_to_index(dx, dy, dtheta) + t = i % self.nb_steps_in_period / self.nb_steps_in_period + t = jp.clip(t, 0.0, 1.0) # safeguard + ret = self.sample_polynomial(t, self.data_array[ix][iy][itheta]) + return ret + + +if __name__ == "__main__": + + PRM = PolyReferenceMotion( + "playground/open_duck_mini_v2/data/polynomial_coefficients.pkl" + ) + vals = [] + select_dim = -1 + for i in range(PRM.nb_steps_in_period): + vals.append(PRM.get_reference_motion(0.0, -0.05, -0.1, i)[select_dim]) + + # plot + import matplotlib.pyplot as plt + import numpy as np + + ts = np.arange(0, PRM.nb_steps_in_period) + plt.plot(ts, vals) + plt.show() diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/poly_reference_motion_numpy.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/poly_reference_motion_numpy.py new file mode 100644 index 0000000..e063271 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/poly_reference_motion_numpy.py @@ -0,0 +1,138 @@ +import numpy as np +import pickle + + +class PolyReferenceMotion: + def __init__(self, polynomial_coefficients: str): + data = pickle.load(open(polynomial_coefficients, "rb")) + self.dx_range = [0, 0] + self.dy_range = [0, 0] + self.dtheta_range = [0, 0] + self.dxs = [] + self.dys = [] + self.dthetas = [] + self.data_array = [] + self.period = None + self.fps = None + self.frame_offsets = None + self.startend_double_support_ratio = None + self.start_offset = None + self.nb_steps_in_period = None + + self.process(data) + + def process(self, data): + print("[Poly ref data] Processing ...") + _data = {} + for name in data.keys(): + split = name.split("_") + dx = float(split[0]) + dy = float(split[1]) + dtheta = float(split[2]) + + if self.period is None: + self.period = data[name]["period"] + self.fps = data[name]["fps"] + self.frame_offsets = data[name]["frame_offsets"] + self.startend_double_support_ratio = data[name][ + "startend_double_support_ratio" + ] + self.start_offset = int(self.startend_double_support_ratio * self.fps) + self.nb_steps_in_period = int(self.period * self.fps) + + if dx not in self.dxs: + self.dxs.append(dx) + + if dy not in self.dys: + self.dys.append(dy) + + if dtheta not in self.dthetas: + self.dthetas.append(dtheta) + + self.dx_range = [min(dx, self.dx_range[0]), max(dx, self.dx_range[1])] + self.dy_range = [min(dy, self.dy_range[0]), max(dy, self.dy_range[1])] + self.dtheta_range = [ + min(dtheta, self.dtheta_range[0]), + max(dtheta, self.dtheta_range[1]), + ] + + if dx not in _data: + _data[dx] = {} + + if dy not in _data[dx]: + _data[dx][dy] = {} + + if dtheta not in _data[dx][dy]: + _data[dx][dy][dtheta] = data[name] + + _coeffs = data[name]["coefficients"] + + coeffs = [] + for k, v in _coeffs.items(): + coeffs.append(v) + _data[dx][dy][dtheta] = coeffs + + self.dxs = sorted(self.dxs) + self.dys = sorted(self.dys) + self.dthetas = sorted(self.dthetas) + + nb_dx = len(self.dxs) + nb_dy = len(self.dys) + nb_dtheta = len(self.dthetas) + + self.data_array = nb_dx * [None] + for x, dx in enumerate(self.dxs): + self.data_array[x] = nb_dy * [None] + for y, dy in enumerate(self.dys): + self.data_array[x][y] = nb_dtheta * [None] + for th, dtheta in enumerate(self.dthetas): + self.data_array[x][y][th] = _data[dx][dy][dtheta] + + self.data_array = self.data_array + + print("[Poly ref data] Done processing") + + def vel_to_index(self, dx, dy, dtheta): + + dx = np.clip(dx, self.dx_range[0], self.dx_range[1]) + dy = np.clip(dy, self.dy_range[0], self.dy_range[1]) + dtheta = np.clip(dtheta, self.dtheta_range[0], self.dtheta_range[1]) + + ix = np.argmin(np.abs(np.array(self.dxs) - dx)) + iy = np.argmin(np.abs(np.array(self.dys) - dy)) + itheta = np.argmin(np.abs(np.array(self.dthetas) - dtheta)) + + return int(ix), int(iy), int(itheta) + + def sample_polynomial(self, t, coeffs): + ret = [] + for c in coeffs: + ret.append(np.polyval(np.flip(c), t)) + + return ret + + def get_reference_motion(self, dx, dy, dtheta, i): + ix, iy, itheta = self.vel_to_index(dx, dy, dtheta) + t = i % self.nb_steps_in_period / self.nb_steps_in_period + t = np.clip(t, 0.0, 1.0) # safeguard + ret = self.sample_polynomial(t, self.data_array[ix][iy][itheta]) + return ret + + +if __name__ == "__main__": + + PRM = PolyReferenceMotion( + "playground/open_duck_mini_v2/data/polynomial_coefficients.pkl" + ) + vals = [] + select_dim = -1 + for i in range(PRM.nb_steps_in_period): + vals.append(PRM.get_reference_motion(0.0, -0.05, -0.1, i)[select_dim]) + + # plot + import matplotlib.pyplot as plt + import numpy as np + + ts = np.arange(0, PRM.nb_steps_in_period) + plt.plot(ts, vals) + plt.show() diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/randomize.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/randomize.py new file mode 100644 index 0000000..cf17bcd --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/randomize.py @@ -0,0 +1,146 @@ +# Copyright 2025 DeepMind Technologies Limited +# Copyright 2025 Antoine Pirrone - Steve Nguyen +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Domain randomization for the Open Duck Mini V2 environment. (based on Berkeley Humanoid)""" + +import jax +from mujoco import mjx +import jax.numpy as jp + +FLOOR_GEOM_ID = 0 +TORSO_BODY_ID = 1 + + +def domain_randomize(model: mjx.Model, rng: jax.Array): + + # _dof_addr=jp.array([6,8,10,12,14,16,18,20,22,24]) + # _joint_addr=jp.array([7,9,11,13,15,17,19,21,23,25]) + + dof_id = jp.array( + [idx for idx, fr in enumerate(model.dof_hasfrictionloss) if fr == True] + ) # for backlash joint we disable frictionloss + jnt_id = model.dof_jntid[dof_id] + + dof_addr = jp.array([jadd for jadd in model.jnt_dofadr if jadd in dof_id]) + joint_addr = model.jnt_qposadr[jnt_id] + + @jax.vmap + def rand_dynamics(rng): + # Floor friction: =U(0.4, 1.0). + rng, key = jax.random.split(rng) + geom_friction = model.geom_friction.at[FLOOR_GEOM_ID, 0].set( + jax.random.uniform(key, minval=0.5, maxval=1.0) # was 0.4, 1.0 + ) + + # Scale static friction: *U(0.9, 1.1). + rng, key = jax.random.split(rng) + frictionloss = model.dof_frictionloss[dof_addr] * jax.random.uniform( + key, shape=(model.nu,), minval=0.9, maxval=1.1 + ) + dof_frictionloss = model.dof_frictionloss.at[dof_addr].set(frictionloss) + + # Scale armature: *U(1.0, 1.05). + rng, key = jax.random.split(rng) + armature = model.dof_armature[dof_addr] * jax.random.uniform( + key, shape=(model.nu,), minval=1.0, maxval=1.05 + ) + dof_armature = model.dof_armature.at[dof_addr].set(armature) + + # Jitter center of mass positiion: +U(-0.05, 0.05). + rng, key = jax.random.split(rng) + dpos = jax.random.uniform(key, (3,), minval=-0.05, maxval=0.05) + body_ipos = model.body_ipos.at[TORSO_BODY_ID].set( + model.body_ipos[TORSO_BODY_ID] + dpos + ) + + # Scale all link masses: *U(0.9, 1.1). + rng, key = jax.random.split(rng) + dmass = jax.random.uniform(key, shape=(model.nbody,), minval=0.9, maxval=1.1) + body_mass = model.body_mass.at[:].set(model.body_mass * dmass) + + # Add mass to torso: +U(-0.2, 0.2). + rng, key = jax.random.split(rng) + dmass = jax.random.uniform(key, minval=-0.1, maxval=0.1) # was -0.2, 0.2 + body_mass = body_mass.at[TORSO_BODY_ID].set(body_mass[TORSO_BODY_ID] + dmass) + + # Jitter qpos0: +U(-0.05, 0.05). + rng, key = jax.random.split(rng) + qpos0 = model.qpos0 + qpos0 = qpos0.at[joint_addr].set( + qpos0[joint_addr] + + jax.random.uniform( + key, shape=(model.nu,), minval=-0.03, maxval=0.03 + ) # was -0.05 0.05 + ) + + # # Randomize KP + rng, key = jax.random.split(rng) + factor = jax.random.uniform( + key, shape=(model.nu,), minval=0.9, maxval=1.1 + ) # was 0.8, 1.2 + current_kp = model.actuator_gainprm[:, 0] + actuator_gainprm = model.actuator_gainprm.at[:, 0].set(current_kp * factor) + actuator_biasprm = model.actuator_biasprm.at[:, 1].set(-current_kp * factor) + + return ( + geom_friction, + body_ipos, + dof_frictionloss, + dof_armature, + body_mass, + qpos0, + actuator_gainprm, + actuator_biasprm, + ) + + ( + friction, + body_ipos, + frictionloss, + armature, + body_mass, + qpos0, + actuator_gainprm, + actuator_biasprm, + ) = rand_dynamics(rng) + + in_axes = jax.tree_util.tree_map(lambda x: None, model) + in_axes = in_axes.tree_replace( + { + "geom_friction": 0, + "body_ipos": 0, + "dof_frictionloss": 0, + "dof_armature": 0, + "body_mass": 0, + "qpos0": 0, + "actuator_gainprm": 0, + "actuator_biasprm": 0, + } + ) + + model = model.tree_replace( + { + "geom_friction": friction, + "body_ipos": body_ipos, + "dof_frictionloss": frictionloss, + "dof_armature": armature, + "body_mass": body_mass, + "qpos0": qpos0, + "actuator_gainprm": actuator_gainprm, + "actuator_biasprm": actuator_biasprm, + } + ) + + return model, in_axes diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/rewards.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/rewards.py new file mode 100644 index 0000000..1700a62 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/rewards.py @@ -0,0 +1,241 @@ +""" +Set of commonly used rewards +For examples on how to use some rewards, look at https://github.com/google-deepmind/mujoco_playground/blob/main/mujoco_playground/_src/locomotion/berkeley_humanoid/joystick.py +""" + +import jax +import jax.numpy as jp + + +# Tracking rewards. +def reward_tracking_lin_vel( + commands: jax.Array, + local_vel: jax.Array, + tracking_sigma: float, +) -> jax.Array: + # lin_vel_error = jp.sum(jp.square(commands[:2] - local_vel[:2])) + # return jp.nan_to_num(jp.exp(-lin_vel_error / self._config.reward_config.tracking_sigma)) + y_tol = 0.1 + error_x = jp.square(commands[0] - local_vel[0]) + error_y = jp.clip(jp.abs(local_vel[1] - commands[1]) - y_tol, 0.0, None) + lin_vel_error = error_x + jp.square(error_y) + return jp.nan_to_num(jp.exp(-lin_vel_error / tracking_sigma)) + + +def reward_tracking_ang_vel( + commands: jax.Array, + ang_vel: jax.Array, + tracking_sigma: float, +) -> jax.Array: + ang_vel_error = jp.square(commands[2] - ang_vel[2]) + return jp.nan_to_num(jp.exp(-ang_vel_error / tracking_sigma)) + + +# Base-related rewards. + + +def cost_lin_vel_z(global_linvel) -> jax.Array: + return jp.nan_to_num(jp.square(global_linvel[2])) + + +def cost_ang_vel_xy(global_angvel) -> jax.Array: + return jp.nan_to_num(jp.sum(jp.square(global_angvel[:2]))) + + +def cost_orientation(torso_zaxis: jax.Array) -> jax.Array: + return jp.nan_to_num(jp.sum(jp.square(torso_zaxis[:2]))) + + +def cost_base_height(base_height: jax.Array, base_height_target: float) -> jax.Array: + return jp.nan_to_num(jp.square(base_height - base_height_target)) + + +def reward_base_y_swing( + base_y_speed: jax.Array, + freq: float, + amplitude: float, + t: float, + tracking_sigma: float, +) -> jax.Array: + target_y_speed = amplitude * jp.sin(2 * jp.pi * freq * t) + y_speed_error = jp.square(target_y_speed - base_y_speed) + return jp.nan_to_num(jp.exp(-y_speed_error / tracking_sigma)) + + +# Energy related rewards. + + +def cost_torques(torques: jax.Array) -> jax.Array: + return jp.nan_to_num(jp.sum(jp.square(torques))) + # return jp.nan_to_num(jp.sum(jp.abs(torques))) + + +def cost_energy(qvel: jax.Array, qfrc_actuator: jax.Array) -> jax.Array: + return jp.nan_to_num(jp.sum(jp.abs(qvel) * jp.abs(qfrc_actuator))) + + +def cost_action_rate(act: jax.Array, last_act: jax.Array) -> jax.Array: + c1 = jp.nan_to_num(jp.sum(jp.square(act - last_act))) + return c1 + + +# Other rewards. + + +def cost_joint_pos_limits( + qpos: jax.Array, soft_lowers: float, soft_uppers: float +) -> jax.Array: + out_of_limits = -jp.clip(qpos - soft_lowers, None, 0.0) + out_of_limits += jp.clip(qpos - soft_uppers, 0.0, None) + return jp.nan_to_num(jp.sum(out_of_limits)) + + +def cost_stand_still( + commands: jax.Array, + qpos: jax.Array, + qvel: jax.Array, + default_pose: jax.Array, + ignore_head: bool = False, +) -> jax.Array: + # TODO no hard coded slices + cmd_norm = jp.linalg.norm(commands[:3]) + if not ignore_head: + pose_cost = jp.sum(jp.abs(qpos - default_pose)) + vel_cost = jp.sum(jp.abs(qvel)) + else: + left_leg_pos = qpos[:5] + right_leg_pos = qpos[9:] + left_leg_vel = qvel[:5] + right_leg_vel = qvel[9:] + left_leg_default = default_pose[:5] + right_leg_default = default_pose[9:] + pose_cost = jp.sum(jp.abs(left_leg_pos - left_leg_default)) + jp.sum( + jp.abs(right_leg_pos - right_leg_default) + ) + vel_cost = jp.sum(jp.abs(left_leg_vel)) + jp.sum(jp.abs(right_leg_vel)) + + return jp.nan_to_num(pose_cost + vel_cost) * (cmd_norm < 0.01) + + +def cost_termination(done: jax.Array) -> jax.Array: + return done + + +def reward_alive() -> jax.Array: + return jp.array(1.0) + + +# Pose-related rewards. + + +def cost_head_pos( + joints_qpos: jax.Array, + joints_qvel: jax.Array, + cmd: jax.Array, +) -> jax.Array: + move_cmd_norm = jp.linalg.norm(cmd[:3]) + head_cmd = cmd[3:] + head_pos = joints_qpos[5:9] + # head_vel = joints_qvel[5:9] + + # target_head_qvel = jp.zeros_like(head_cmd) + + head_pos_error = jp.sum(jp.square(head_pos - head_cmd)) + + # head_vel_error = jp.sum(jp.square(head_vel - target_head_qvel)) + + return jp.nan_to_num(head_pos_error) * (move_cmd_norm > 0.01) + # return jp.nan_to_num(head_pos_error + head_vel_error) + + +# FIXME +def cost_joint_deviation_hip( + qpos: jax.Array, cmd: jax.Array, hip_indices: jax.Array, default_pose: jax.Array +) -> jax.Array: + cost = jp.sum(jp.abs(qpos[hip_indices] - default_pose[hip_indices])) + cost *= jp.abs(cmd[1]) > 0.1 + return jp.nan_to_num(cost) + + +# FIXME +def cost_joint_deviation_knee( + qpos: jax.Array, knee_indices: jax.Array, default_pose: jax.Array +) -> jax.Array: + return jp.nan_to_num( + jp.sum(jp.abs(qpos[knee_indices] - default_pose[knee_indices])) + ) + + +# FIXME +def cost_pose( + qpos: jax.Array, default_pose: jax.Array, weights: jax.Array +) -> jax.Array: + return jp.nan_to_num(jp.sum(jp.square(qpos - default_pose) * weights)) + + +# Feet related rewards. + + +# FIXME +def cost_feet_slip(contact: jax.Array, global_linvel: jax.Array) -> jax.Array: + body_vel = global_linvel[:2] + reward = jp.sum(jp.linalg.norm(body_vel, axis=-1) * contact) + return jp.nan_to_num(reward) + + +# FIXME +def cost_feet_clearance( + feet_vel: jax.Array, + foot_pos: jax.Array, + max_foot_height: float, +) -> jax.Array: + # feet_vel = data.sensordata[self._foot_linvel_sensor_adr] + vel_xy = feet_vel[..., :2] + vel_norm = jp.sqrt(jp.linalg.norm(vel_xy, axis=-1)) + # foot_pos = data.site_xpos[self._feet_site_id] + foot_z = foot_pos[..., -1] + delta = jp.abs(foot_z - max_foot_height) + return jp.nan_to_num(jp.sum(delta * vel_norm)) + + +# FIXME +def cost_feet_height( + swing_peak: jax.Array, + first_contact: jax.Array, + max_foot_height: float, +) -> jax.Array: + error = swing_peak / max_foot_height - 1.0 + return jp.nan_to_num(jp.sum(jp.square(error) * first_contact)) + + +# FIXME +def reward_feet_air_time( + air_time: jax.Array, + first_contact: jax.Array, + commands: jax.Array, + threshold_min: float = 0.1, # 0.2 + threshold_max: float = 0.5, +) -> jax.Array: + cmd_norm = jp.linalg.norm(commands[:3]) + air_time = (air_time - threshold_min) * first_contact + air_time = jp.clip(air_time, max=threshold_max - threshold_min) + reward = jp.sum(air_time) + reward *= cmd_norm > 0.01 # No reward for zero commands. + return jp.nan_to_num(reward) + + +# FIXME +def reward_feet_phase( + foot_pos: jax.Array, + rz: jax.Array, +) -> jax.Array: + # Reward for tracking the desired foot height. + # foot_pos = data.site_xpos[self._feet_site_id] + foot_z = foot_pos[..., -1] + # rz = gait.get_rz(phase, swing_height=foot_height) + error = jp.sum(jp.square(foot_z - rz)) + reward = jp.exp(-error / 0.01) + # TODO(kevin): Ensure no movement at 0 command. + # cmd_norm = jp.linalg.norm(commands) + # reward *= cmd_norm > 0.1 # No reward for zero commands. + return jp.nan_to_num(reward) diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/rewards_numpy.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/rewards_numpy.py new file mode 100644 index 0000000..b5198c8 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/rewards_numpy.py @@ -0,0 +1,196 @@ +""" +Set of commonly used rewards +For examples on how to use some rewards, look at https://github.com/google-deepmind/mujoco_playground/blob/main/mujoco_playground/_src/locomotion/berkeley_humanoid/joystick.py +""" + +# import jax +# import jax.numpy as np + +import numpy as np + + +# Tracking rewards. +def reward_tracking_lin_vel(commands, local_vel, tracking_sigma): + # lin_vel_error = np.sum(np.square(commands[:2] - local_vel[:2])) + # return np.nan_to_num(np.exp(-lin_vel_error / self._config.reward_config.tracking_sigma)) + y_tol = 0.1 + error_x = np.square(commands[0] - local_vel[0]) + error_y = np.clip(np.abs(local_vel[1] - commands[1]) - y_tol, 0.0, None) + lin_vel_error = error_x + np.square(error_y) + return np.nan_to_num(np.exp(-lin_vel_error / tracking_sigma)) + + +def reward_tracking_ang_vel(commands, ang_vel, tracking_sigma): + ang_vel_error = np.square(commands[2] - ang_vel[2]) + return np.nan_to_num(np.exp(-ang_vel_error / tracking_sigma)) + + +# Base-related rewards. + + +def cost_lin_vel_z(global_linvel): + return np.nan_to_num(np.square(global_linvel[2])) + + +def cost_ang_vel_xy(global_angvel): + return np.nan_to_num(np.sum(np.square(global_angvel[:2]))) + + +def cost_orientation(torso_zaxis): + return np.nan_to_num(np.sum(np.square(torso_zaxis[:2]))) + + +def cost_base_height(base_height, base_height_target): + return np.nan_to_num(np.square(base_height - base_height_target)) + + +def reward_base_y_swing(base_y_speed, freq, amplitude, t, tracking_sigma): + target_y_speed = amplitude * np.sin(2 * np.pi * freq * t) + y_speed_error = np.square(target_y_speed - base_y_speed) + return np.nan_to_num(np.exp(-y_speed_error / tracking_sigma)) + + +# Energy related rewards. + + +def cost_torques(torques): + return np.nan_to_num(np.sum(np.square(torques))) + # return np.nan_to_num(np.sum(np.abs(torques))) + + +def cost_energy(qvel, qfrc_actuator): + return np.nan_to_num(np.sum(np.abs(qvel) * np.abs(qfrc_actuator))) + + +def cost_action_rate(act, last_act): + c1 = np.nan_to_num(np.sum(np.square(act - last_act))) + return c1 + + +# Other rewards. + + +def cost_joint_pos_limits(qpos, soft_lowers, soft_uppers): + out_of_limits = -np.clip(qpos - soft_lowers, None, 0.0) + out_of_limits += np.clip(qpos - soft_uppers, 0.0, None) + return np.nan_to_num(np.sum(out_of_limits)) + + +def cost_stand_still(commands, qpos, qvel, default_pose, ignore_head=False): + # TODO no hard coded slices + cmd_norm = np.linalg.norm(commands[:3]) + if not ignore_head: + pose_cost = np.sum(np.abs(qpos - default_pose)) + vel_cost = np.sum(np.abs(qvel)) + else: + left_leg_pos = qpos[:5] + right_leg_pos = qpos[9:] + left_leg_vel = qvel[:5] + right_leg_vel = qvel[9:] + left_leg_default = default_pose[:5] + right_leg_default = default_pose[9:] + pose_cost = np.sum(np.abs(left_leg_pos - left_leg_default)) + np.sum( + np.abs(right_leg_pos - right_leg_default) + ) + vel_cost = np.sum(np.abs(left_leg_vel)) + np.sum(np.abs(right_leg_vel)) + + return np.nan_to_num(pose_cost + vel_cost) * (cmd_norm < 0.01) + + +def cost_termination(done): + return done + + +def reward_alive(): + return np.array(1.0) + + +# Pose-related rewards. + + +def cost_head_pos(joints_qpos, joints_qvel, cmd): + move_cmd_norm = np.linalg.norm(cmd[:3]) + head_cmd = cmd[3:] + head_pos = joints_qpos[5:9] + # head_vel = joints_qvel[5:9] + + # target_head_qvel = np.zeros_like(head_cmd) + + head_pos_error = np.sum(np.square(head_pos - head_cmd)) + + # head_vel_error = np.sum(np.square(head_vel - target_head_qvel)) + + return np.nan_to_num(head_pos_error) * (move_cmd_norm > 0.01) + # return np.nan_to_num(head_pos_error + head_vel_error) + + +# FIXME +def cost_joint_deviation_hip(qpos, cmd, hip_indices, default_pose): + cost = np.sum(np.abs(qpos[hip_indices] - default_pose[hip_indices])) + cost *= np.abs(cmd[1]) > 0.1 + return np.nan_to_num(cost) + + +# FIXME +def cost_joint_deviation_knee(qpos, knee_indices, default_pose): + return np.nan_to_num( + np.sum(np.abs(qpos[knee_indices] - default_pose[knee_indices])) + ) + + +# FIXME +def cost_pose(qpos, default_pose, weights): + return np.nan_to_num(np.sum(np.square(qpos - default_pose) * weights)) + + +# Feet related rewards. + + +# FIXME +def cost_feet_slip(contact, global_linvel): + body_vel = global_linvel[:2] + reward = np.sum(np.linalg.norm(body_vel, axis=-1) * contact) + return np.nan_to_num(reward) + + +# FIXME +def cost_feet_clearance(feet_vel, foot_pos, max_foot_height): + # feet_vel = data.sensordata[self._foot_linvel_sensor_adr] + vel_xy = feet_vel[..., :2] + vel_norm = np.sqrt(np.linalg.norm(vel_xy, axis=-1)) + # foot_pos = data.site_xpos[self._feet_site_id] + foot_z = foot_pos[..., -1] + delta = np.abs(foot_z - max_foot_height) + return np.nan_to_num(np.sum(delta * vel_norm)) + + +# FIXME +def cost_feet_height(swing_peak, first_contact, max_foot_height): + error = swing_peak / max_foot_height - 1.0 + return np.nan_to_num(np.sum(np.square(error) * first_contact)) + + +# FIXME +def reward_feet_air_time( + air_time, first_contact, commands, threshold_min=0.1, threshold_max=0.5 # 0.2 +): + cmd_norm = np.linalg.norm(commands[:3]) + air_time = (air_time - threshold_min) * first_contact + air_time = np.clip(air_time, max=threshold_max - threshold_min) + reward = np.sum(air_time) + reward *= cmd_norm > 0.01 # No reward for zero commands. + return np.nan_to_num(reward) + + +# FIXME +def reward_feet_phase(foot_pos, rz): + # Reward for tracking the desired foot height. + # foot_pos = data.site_xpos[self._feet_site_id] + foot_z = foot_pos[..., -1] + # rz = gait.get_rz(phase, swing_height=foot_height) + error = np.sum(np.square(foot_z - rz)) + reward = np.exp(-error / 0.01) + # TODO(kevin): Ensure no movement at 0 command. + # cmd_norm = np.linalg.norm(commands) + # reward *= cmd_norm > 0.1 # No reward for zero commands. + return np.nan_to_num(reward) diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/runner.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/runner.py new file mode 100644 index 0000000..628f325 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/runner.py @@ -0,0 +1,118 @@ +""" +Defines a common runner between the different robots. +Inspired from https://github.com/kscalelabs/mujoco_playground/blob/master/playground/common/runner.py +""" + +from pathlib import Path +from abc import ABC +import argparse +import functools +from datetime import datetime +from flax.training import orbax_utils +from tensorboardX import SummaryWriter + +import os +from brax.training.agents.ppo import networks as ppo_networks, train as ppo +from mujoco_playground import wrapper +from mujoco_playground.config import locomotion_params +from orbax import checkpoint as ocp +import jax + +from playground.common.export_onnx import export_onnx + + +class BaseRunner(ABC): + def __init__(self, args: argparse.Namespace) -> None: + """Initialize the Runner class. + + Args: + args (argparse.Namespace): Command line arguments. + """ + self.args = args + self.output_dir = args.output_dir + self.output_dir = Path.cwd() / Path(self.output_dir) + + self.env_config = None + self.env = None + self.eval_env = None + self.randomizer = None + self.writer = SummaryWriter(log_dir=self.output_dir) + self.action_size = None + self.obs_size = None + self.num_timesteps = args.num_timesteps + self.restore_checkpoint_path = None + + # CACHE STUFF + os.makedirs(".tmp", exist_ok=True) + jax.config.update("jax_compilation_cache_dir", ".tmp/jax_cache") + jax.config.update("jax_persistent_cache_min_entry_size_bytes", -1) + jax.config.update("jax_persistent_cache_min_compile_time_secs", 0) + jax.config.update( + "jax_persistent_cache_enable_xla_caches", + "xla_gpu_per_fusion_autotune_cache_dir", + ) + os.environ["JAX_COMPILATION_CACHE_DIR"] = ".tmp/jax_cache" + + def progress_callback(self, num_steps: int, metrics: dict) -> None: + + for metric_name, metric_value in metrics.items(): + # Convert to float, but watch out for 0-dim JAX arrays + self.writer.add_scalar(metric_name, metric_value, num_steps) + + print("-----------") + print( + f'STEP: {num_steps} reward: {metrics["eval/episode_reward"]} reward_std: {metrics["eval/episode_reward_std"]}' + ) + print("-----------") + + def policy_params_fn(self, current_step, make_policy, params): + # save checkpoints + + orbax_checkpointer = ocp.PyTreeCheckpointer() + save_args = orbax_utils.save_args_from_target(params) + d = datetime.now().strftime("%Y_%m_%d_%H%M%S") + path = f"{self.output_dir}/{d}_{current_step}" + print(f"Saving checkpoint (step: {current_step}): {path}") + orbax_checkpointer.save(path, params, force=True, save_args=save_args) + onnx_export_path = f"{self.output_dir}/{d}_{current_step}.onnx" + export_onnx( + params, + self.action_size, + self.ppo_params, + self.obs_size, # may not work + output_path=onnx_export_path + ) + + def train(self) -> None: + self.ppo_params = locomotion_params.brax_ppo_config( + "BerkeleyHumanoidJoystickFlatTerrain" + ) # TODO + self.ppo_training_params = dict(self.ppo_params) + # self.ppo_training_params["num_timesteps"] = 150000000 * 20 + + + if "network_factory" in self.ppo_params: + network_factory = functools.partial( + ppo_networks.make_ppo_networks, **self.ppo_params.network_factory + ) + del self.ppo_training_params["network_factory"] + else: + network_factory = ppo_networks.make_ppo_networks + self.ppo_training_params["num_timesteps"] = self.num_timesteps + print(f"PPO params: {self.ppo_training_params}") + + train_fn = functools.partial( + ppo.train, + **self.ppo_training_params, + network_factory=network_factory, + randomization_fn=self.randomizer, + progress_fn=self.progress_callback, + policy_params_fn=self.policy_params_fn, + restore_checkpoint_path=self.restore_checkpoint_path, + ) + + _, params, _ = train_fn( + environment=self.env, + eval_env=self.eval_env, + wrap_env_fn=wrapper.wrap_for_brax_training, + ) diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/utils.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/utils.py new file mode 100644 index 0000000..ac387bf --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/common/utils.py @@ -0,0 +1,25 @@ +import jax.numpy as jp +import jax + + +class LowPassActionFilter: + def __init__(self, control_freq, cutoff_frequency=30.0): + self.last_action = 0 + self.current_action = 0 + self.control_freq = float(control_freq) + self.cutoff_frequency = float(cutoff_frequency) + self.alpha = self.compute_alpha() + + def compute_alpha(self): + return (1.0 / self.cutoff_frequency) / ( + 1.0 / self.control_freq + 1.0 / self.cutoff_frequency + ) + + def push(self, action: jax.Array) -> None: + self.current_action = jp.array(action) + + def get_filtered_action(self) -> jax.Array: + self.last_action = ( + self.alpha * self.last_action + (1 - self.alpha) * self.current_action + ) + return self.last_action \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/__init__.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/base.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/base.py new file mode 100644 index 0000000..d269f8c --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/base.py @@ -0,0 +1,291 @@ +# Copyright 2025 DeepMind Technologies Limited +# Copyright 2025 Antoine Pirrone - Steve Nguyen +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Base classes for Open Duck Mini V2. (based on Berkeley Humanoid)""" + +from typing import Any, Dict, Optional, Union + +from etils import epath +import jax +import jax.numpy as jp +from ml_collections import config_dict +import mujoco +from mujoco import mjx + +from mujoco_playground._src import mjx_env +from . import constants + + +def get_assets() -> Dict[str, bytes]: + assets = {} + mjx_env.update_assets(assets, constants.ROOT_PATH / "xmls", "*.xml") + mjx_env.update_assets(assets, constants.ROOT_PATH / "xmls" / "assets") + path = constants.ROOT_PATH + mjx_env.update_assets(assets, path, "*.xml") + mjx_env.update_assets(assets, path / "assets") + return assets + + +class OpenDuckMiniV2Env(mjx_env.MjxEnv): + """Base class for Open Duck Mini V2 environments.""" + + def __init__( + self, + xml_path: str, + config: config_dict.ConfigDict, + config_overrides: Optional[Dict[str, Union[str, int, list[Any]]]] = None, + ) -> None: + super().__init__(config, config_overrides) + + print(f"xml: {xml_path}") + self._mj_model = mujoco.MjModel.from_xml_string( + epath.Path(xml_path).read_text(), assets=get_assets() + ) + self._mj_model.opt.timestep = self.sim_dt + + self._mj_model.vis.global_.offwidth = 3840 + self._mj_model.vis.global_.offheight = 2160 + + self._mjx_model = mjx.put_model(self._mj_model) + self._xml_path = xml_path + self.floating_base_name= [self._mj_model.jnt(k).name for k in range(0, self._mj_model.njnt) if self._mj_model.jnt(k).type == 0][0] #assuming only one floating object! + self.actuator_names = [ + self._mj_model.actuator(k).name for k in range(0, self._mj_model.nu) + ] # will be useful to get only the actuators we care about + self.joint_names = [ #njnt = all joints (including floating base, actuators and backlash joints) + self._mj_model.jnt(k).name for k in range(0, self._mj_model.njnt) + ] # all the joint (including the backlash joints) + self.backlash_joint_names = [ + j for j in self.joint_names if j not in self.actuator_names and j not in self.floating_base_name + ] # only the dummy backlash joint + self.all_joint_ids = [self.get_joint_id_from_name(n) for n in self.joint_names] + self.all_joint_qpos_addr = [self.get_joint_addr_from_name(n) for n in self.joint_names] + + self.actuator_joint_ids = [ + self.get_joint_id_from_name(n) for n in self.actuator_names + ] + self.actuator_joint_qpos_addr = [ + self.get_joint_addr_from_name(n) for n in self.actuator_names + ] + + self.backlash_joint_ids=[ + self.get_joint_id_from_name(n) for n in self.backlash_joint_names + ] + + self.backlash_joint_qpos_addr=[ + self.get_joint_addr_from_name(n) for n in self.backlash_joint_names + ] + + self.all_qvel_addr=jp.array([self._mj_model.jnt_dofadr[jad] for jad in self.all_joint_ids]) + self.actuator_qvel_addr=jp.array([self._mj_model.jnt_dofadr[jad] for jad in self.actuator_joint_ids]) + + self.actuator_joint_dict = { + n: self.get_joint_id_from_name(n) for n in self.actuator_names + } + + self._floating_base_qpos_addr = self._mj_model.jnt_qposadr[ + jp.where(self._mj_model.jnt_type == 0) + ][ + 0 + ] # Assuming there is only one floating base! the jnt_type==0 is a floating joint. 3 is a hinge + + self._floating_base_qvel_addr = self._mj_model.jnt_dofadr[ + jp.where(self._mj_model.jnt_type == 0) + ][ + 0 + ] # Assuming there is only one floating base! the jnt_type==0 is a floating joint. 3 is a hinge + + self._floating_base_id = self._mj_model.joint(self.floating_base_name).id + + # self.all_joint_no_backlash_ids=jp.zeros(7+self._mj_model.nu) + # all_idx=self.actuator_joint_ids+list(range(self._floating_base_qpos_addr,self._floating_base_qpos_addr+7)) + # all_idx=jp.array(all_idx).sort() + all_idx=self.actuator_joint_ids+list([self.get_joint_id_from_name("trunk_assembly_freejoint")]) + all_idx=jp.array(all_idx).sort() + # self.all_joint_no_backlash_ids=[idx for idx in self.all_joint_ids if idx not in self.backlash_joint_ids]+list(range(self._floating_base_add,self._floating_base_add+7)) + self.all_joint_no_backlash_ids=[idx for idx in all_idx] + # print(f"ALL: {self.all_joint_no_backlash_ids} back_id: {self.backlash_joint_ids} base_id: {list(range(self._floating_base_qpos_addr,self._floating_base_qpos_addr+7))}") + + self.backlash_idx_to_add = [] + + for i, actuator_name in enumerate(self.actuator_names): + if actuator_name + "_backlash" not in self.backlash_joint_names: + self.backlash_idx_to_add.append(i) + + print(f"actuators: {self.actuator_names}") + print(f"joints: {self.joint_names}") + print(f"backlash joints: {self.backlash_joint_names}") + print(f"actuator joints ids: {self.actuator_joint_ids}") + print(f"actuator joints dict: {self.actuator_joint_dict}") + print(f"floating qpos addr: {self._floating_base_qpos_addr} qvel addr: {self._floating_base_qvel_addr}") + + + + def get_actuator_id_from_name(self, name: str) -> int: + """Return the id of a specified actuator""" + return mujoco.mj_name2id(self._mj_model, mujoco.mjtObj.mjOBJ_ACTUATOR, name) + + def get_joint_id_from_name(self, name: str) -> int: + """Return the id of a specified joint""" + return mujoco.mj_name2id(self._mj_model, mujoco.mjtObj.mjOBJ_JOINT, name) + + + def get_joint_addr_from_name(self, name: str) -> int: + """Return the address of a specified joint""" + return self._mj_model.joint(name).qposadr + + def get_dof_id_from_name(self, name: str) -> int: + """Return the id of a specified dof""" + return mujoco.mj_name2id(self._mj_model, mujoco.mjtObj.mjOBJ_DOF, name) + + + def get_actuator_joint_qpos_from_name(self, data: jax.Array, name: str) -> jax.Array: + """Return the qpos of a given actual joint""" + addr = self._mj_model.jnt_qposadr[self.actuator_joint_dict[name]] + return data[addr] + + def get_actuator_joints_qpos_addr(self) -> jax.Array: + """Return the all the idx of actual joints""" + addr = jp.array( + [self._mj_model.jnt_qposadr[idx] for idx in self.actuator_joint_ids] + ) + return addr + + def get_floating_base_qpos(self, data:jax.Array) -> jax.Array: + return data[self._floating_base_qpos_addr:self._floating_base_qvel_addr+7] + + def get_floating_base_qvel(self, data:jax.Array) -> jax.Array: + return data[self._floating_base_qvel_addr:self._floating_base_qvel_addr+6] + + + def set_floating_base_qpos(self, new_qpos:jax.Array, qpos:jax.Array) -> jax.Array: + return qpos.at[self._floating_base_qpos_addr:self._floating_base_qpos_addr+7].set(new_qpos) + + def set_floating_base_qvel(self, new_qvel:jax.Array, qvel:jax.Array) -> jax.Array: + return qvel.at[self._floating_base_qvel_addr:self._floating_base_qvel_addr+6].set(new_qvel) + + + def exclude_backlash_joints_addr(self) -> jax.Array: + """Return the all the idx of actual joints and floating base""" + addr = jp.array( + [self._mj_model.jnt_qposadr[idx] for idx in self.all_joint_no_backlash_ids] + ) + return addr + + + def get_all_joints_addr(self) -> jax.Array: + """Return the all the idx of all joints""" + addr = jp.array([self._mj_model.jnt_qposadr[idx] for idx in self.all_joint_ids]) + return addr + + def get_actuator_joints_qpos(self, data: jax.Array) -> jax.Array: + """Return the all the qpos of actual joints""" + return data[self.get_actuator_joints_qpos_addr()] + + def set_actuator_joints_qpos(self, new_qpos: jax.Array, qpos: jax.Array) -> jax.Array: + """Set the qpos only for the actual joints (omit the backlash joint)""" + return qpos.at[self.get_actuator_joints_qpos_addr()].set(new_qpos) + + def get_actuator_backlash_qpos(self, data: jax.Array) -> jax.Array: + """Return the all the qpos of backlash joints""" + if self.backlash_joint_qpos_addr == []: + return jp.array([]) + return data[jp.array(self.backlash_joint_qpos_addr)] + + + def get_actuator_joints_qvel(self, data: jax.Array) -> jax.Array: + """Return the all the qvel of actual joints""" + return data[self.actuator_qvel_addr] + + def set_actuator_joints_qvel(self, new_qvel: jax.Array, qvel: jax.Array) -> jax.Array: + """Set the qvel only for the actual joints (omit the backlash joint)""" + return qvel.at[self.actuator_qvel_addr].set(new_qvel) + + def get_all_joints_qpos(self, data: jax.Array) -> jax.Array: + """Return the all the qpos of all joints""" + return data[self.get_all_joints_addr()] + + def get_all_joints_qvel(self, data: jax.Array) -> jax.Array: + """Return the all the qvel of all joints""" + return data[self.all_qvel_addr] + + def get_joints_nobacklash_qpos(self, data: jax.Array) -> jax.Array: + """Return the all the qpos of actual joints with the floating base""" + return data[self.exclude_backlash_joints_addr()] + + def set_complete_qpos_from_joints(self, no_backlash_qpos: jax.Array, full_qpos: jax.Array) -> jax.Array: + """In the case of backlash joints, we want to ignore them (remove them) but we still need to set the complete state incuding them""" + full_qpos.at[self.exclude_backlash_joints_addr()].set(no_backlash_qpos) + return jp.array(full_qpos) + + # Sensor readings. + def get_gravity(self, data: mjx.Data) -> jax.Array: + """Return the gravity vector in the world frame.""" + return mjx_env.get_sensor_data(self.mj_model, data, constants.GRAVITY_SENSOR) + + def get_global_linvel(self, data: mjx.Data) -> jax.Array: + """Return the linear velocity of the robot in the world frame.""" + return mjx_env.get_sensor_data( + self.mj_model, data, constants.GLOBAL_LINVEL_SENSOR + ) + + def get_global_angvel(self, data: mjx.Data) -> jax.Array: + """Return the angular velocity of the robot in the world frame.""" + return mjx_env.get_sensor_data( + self.mj_model, data, constants.GLOBAL_ANGVEL_SENSOR + ) + + def get_local_linvel(self, data: mjx.Data) -> jax.Array: + """Return the linear velocity of the robot in the local frame.""" + return mjx_env.get_sensor_data( + self.mj_model, data, constants.LOCAL_LINVEL_SENSOR + ) + + def get_accelerometer(self, data: mjx.Data) -> jax.Array: + """Return the accelerometer readings in the local frame.""" + return mjx_env.get_sensor_data( + self.mj_model, data, constants.ACCELEROMETER_SENSOR + ) + + def get_gyro(self, data: mjx.Data) -> jax.Array: + """Return the gyroscope readings in the local frame.""" + return mjx_env.get_sensor_data(self.mj_model, data, constants.GYRO_SENSOR) + + def get_feet_pos(self, data: mjx.Data) -> jax.Array: + """Return the position of the feet in the world frame.""" + return jp.vstack( + [ + mjx_env.get_sensor_data(self.mj_model, data, sensor_name) + for sensor_name in constants.FEET_POS_SENSOR + ] + ) + + # Accessors. + + @property + def xml_path(self) -> str: + return self._xml_path + + @property + def action_size(self) -> int: + return self._mjx_model.nu + + @property + def mj_model(self) -> mujoco.MjModel: + return self._mj_model + + @property + def mjx_model(self) -> mjx.Model: + return self._mjx_model diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/constants.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/constants.py new file mode 100644 index 0000000..821c1fc --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/constants.py @@ -0,0 +1,89 @@ +# Copyright 2025 DeepMind Technologies Limited +# Copyright 2025 Antoine Pirrone - Steve Nguyen +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Constants for Open Duck Mini V2. (based on Berkeley Humanoid)""" + +from etils import epath + + +ROOT_PATH = epath.Path(__file__).parent +FLAT_TERRAIN_XML = ROOT_PATH / "xmls" / "scene_flat_terrain.xml" +ROUGH_TERRAIN_XML = ROOT_PATH / "xmls" / "scene_rough_terrain.xml" +FLAT_TERRAIN_BACKLASH_XML = ROOT_PATH / "xmls" / "scene_flat_terrain_backlash.xml" +ROUGH_TERRAIN_BACKLASH_XML = ROOT_PATH / "xmls" / "scene_rough_terrain_backlash.xml" + + +def task_to_xml(task_name: str) -> epath.Path: + return { + "flat_terrain": FLAT_TERRAIN_XML, + "rough_terrain": ROUGH_TERRAIN_XML, + "flat_terrain_backlash": FLAT_TERRAIN_BACKLASH_XML, + "rough_terrain_backlash": ROUGH_TERRAIN_BACKLASH_XML, + }[task_name] + + +FEET_SITES = [ + "left_foot", + "right_foot", +] + +LEFT_FEET_GEOMS = [ + "left_foot_bottom_tpu", +] + +RIGHT_FEET_GEOMS = [ + "right_foot_bottom_tpu", +] + +HIP_JOINT_NAMES = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", +] + +KNEE_JOINT_NAMES = [ + "left_knee", + "right_knee", +] + +# There should be a way to get that from the mjModel... +JOINTS_ORDER_NO_HEAD = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", +] + +FEET_GEOMS = LEFT_FEET_GEOMS + RIGHT_FEET_GEOMS + +FEET_POS_SENSOR = [f"{site}_pos" for site in FEET_SITES] + +ROOT_BODY = "trunk_assembly" + +GRAVITY_SENSOR = "upvector" +GLOBAL_LINVEL_SENSOR = "global_linvel" +GLOBAL_ANGVEL_SENSOR = "global_angvel" +LOCAL_LINVEL_SENSOR = "local_linvel" +ACCELEROMETER_SENSOR = "accelerometer" +GYRO_SENSOR = "gyro" diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/custom_rewards.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/custom_rewards.py new file mode 100644 index 0000000..6b9fcdb --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/custom_rewards.py @@ -0,0 +1,149 @@ +import jax +import jax.numpy as jp + +def reward_imitation( + base_qpos: jax.Array, + base_qvel: jax.Array, + joints_qpos: jax.Array, + joints_qvel: jax.Array, + contacts: jax.Array, + reference_frame: jax.Array, + cmd: jax.Array, + use_imitation_reward: bool = False, +) -> jax.Array: + if not use_imitation_reward: + return jp.nan_to_num(0.0) + + # TODO don't reward for moving when the command is zero. + cmd_norm = jp.linalg.norm(cmd[:3]) + + w_torso_pos = 1.0 + w_torso_orientation = 1.0 + w_lin_vel_xy = 1.0 + w_lin_vel_z = 1.0 + w_ang_vel_xy = 0.5 + w_ang_vel_z = 0.5 + w_joint_pos = 15.0 + w_joint_vel = 1.0e-3 + w_contact = 1.0 + + # TODO : double check if the slices are correct + linear_vel_slice_start = 34 + linear_vel_slice_end = 37 + + angular_vel_slice_start = 37 + angular_vel_slice_end = 40 + + joint_pos_slice_start = 0 + joint_pos_slice_end = 16 + + joint_vels_slice_start = 16 + joint_vels_slice_end = 32 + + # root_pos_slice_start = 0 + # root_pos_slice_end = 3 + + root_quat_slice_start = 3 + root_quat_slice_end = 7 + + # left_toe_pos_slice_start = 23 + # left_toe_pos_slice_end = 26 + + # right_toe_pos_slice_start = 26 + # right_toe_pos_slice_end = 29 + + foot_contacts_slice_start = 32 + foot_contacts_slice_end = 34 + + # ref_base_pos = reference_frame[root_pos_slice_start:root_pos_slice_end] + # base_pos = qpos[:3] + + ref_base_orientation_quat = reference_frame[ + root_quat_slice_start:root_quat_slice_end + ] + ref_base_orientation_quat = ref_base_orientation_quat / jp.linalg.norm( + ref_base_orientation_quat + ) # normalize the quat + base_orientation = base_qpos[3:7] + base_orientation = base_orientation / jp.linalg.norm( + base_orientation + ) # normalize the quat + + ref_base_lin_vel = reference_frame[linear_vel_slice_start:linear_vel_slice_end] + base_lin_vel = base_qvel[:3] + + ref_base_ang_vel = reference_frame[angular_vel_slice_start:angular_vel_slice_end] + base_ang_vel = base_qvel[3:6] + + ref_joint_pos = reference_frame[joint_pos_slice_start:joint_pos_slice_end] + # remove neck head and antennas + ref_joint_pos = jp.concatenate([ref_joint_pos[:5], ref_joint_pos[11:]]) + # joint_pos = joints_qpos + joint_pos = jp.concatenate([joints_qpos[:5], joints_qpos[9:]]) + + ref_joint_vels = reference_frame[joint_vels_slice_start:joint_vels_slice_end] + # remove neck head and antennas + ref_joint_vels = jp.concatenate([ref_joint_vels[:5], ref_joint_vels[11:]]) + # joint_vel = joints_qvel + joint_vel = jp.concatenate([joints_qvel[:5], joints_qvel[9:]]) + + # ref_left_toe_pos = reference_frame[left_toe_pos_slice_start:left_toe_pos_slice_end] + # ref_right_toe_pos = reference_frame[right_toe_pos_slice_start:right_toe_pos_slice_end] + + ref_foot_contacts = reference_frame[ + foot_contacts_slice_start:foot_contacts_slice_end + ] + + # reward + # torso_pos_rew = jp.exp(-200.0 * jp.sum(jp.square(base_pos[:2] - ref_base_pos[:2]))) * w_torso_pos + + # real quaternion angle doesn't have the expected effect, switching back for now + # torso_orientation_rew = jp.exp(-20 * self.quaternion_angle(base_orientation, ref_base_orientation_quat)) * w_torso_orientation + + # TODO ignore yaw here, we just want xy orientation + torso_orientation_rew = ( + jp.exp(-20.0 * jp.sum(jp.square(base_orientation - ref_base_orientation_quat))) + * w_torso_orientation + ) + + lin_vel_xy_rew = ( + jp.exp(-8.0 * jp.sum(jp.square(base_lin_vel[:2] - ref_base_lin_vel[:2]))) + * w_lin_vel_xy + ) + lin_vel_z_rew = ( + jp.exp(-8.0 * jp.sum(jp.square(base_lin_vel[2] - ref_base_lin_vel[2]))) + * w_lin_vel_z + ) + + ang_vel_xy_rew = ( + jp.exp(-2.0 * jp.sum(jp.square(base_ang_vel[:2] - ref_base_ang_vel[:2]))) + * w_ang_vel_xy + ) + ang_vel_z_rew = ( + jp.exp(-2.0 * jp.sum(jp.square(base_ang_vel[2] - ref_base_ang_vel[2]))) + * w_ang_vel_z + ) + + joint_pos_rew = -jp.sum(jp.square(joint_pos - ref_joint_pos)) * w_joint_pos + joint_vel_rew = -jp.sum(jp.square(joint_vel - ref_joint_vels)) * w_joint_vel + + ref_foot_contacts = jp.where( + ref_foot_contacts > 0.5, + jp.ones_like(ref_foot_contacts), + jp.zeros_like(ref_foot_contacts), + ) + contact_rew = jp.sum(contacts == ref_foot_contacts) * w_contact + + reward = ( + lin_vel_xy_rew + + lin_vel_z_rew + + ang_vel_xy_rew + + ang_vel_z_rew + + joint_pos_rew + + joint_vel_rew + + contact_rew + # + torso_orientation_rew + ) + + reward *= cmd_norm > 0.01 # No reward for zero commands. + return jp.nan_to_num(reward) \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/custom_rewards_numpy.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/custom_rewards_numpy.py new file mode 100644 index 0000000..135186f --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/custom_rewards_numpy.py @@ -0,0 +1,151 @@ +# import jax +# import jax.numpy as np +import numpy as np + + +def reward_imitation( + base_qpos, + base_qvel, + joints_qpos, + joints_qvel, + contacts, + reference_frame, + cmd, + use_imitation_reward=False, +): + if not use_imitation_reward: + return np.nan_to_num(0.0) + + # TODO don't reward for moving when the command is zero. + cmd_norm = np.linalg.norm(cmd[:3]) + + w_torso_pos = 1.0 + w_torso_orientation = 1.0 + w_lin_vel_xy = 1.0 + w_lin_vel_z = 1.0 + w_ang_vel_xy = 0.5 + w_ang_vel_z = 0.5 + w_joint_pos = 15.0 + w_joint_vel = 1.0e-3 + w_contact = 1.0 + + # TODO : double check if the slices are correct + linear_vel_slice_start = 34 + linear_vel_slice_end = 37 + + angular_vel_slice_start = 37 + angular_vel_slice_end = 40 + + joint_pos_slice_start = 0 + joint_pos_slice_end = 16 + + joint_vels_slice_start = 16 + joint_vels_slice_end = 32 + + # root_pos_slice_start = 0 + # root_pos_slice_end = 3 + + root_quat_slice_start = 3 + root_quat_slice_end = 7 + + # left_toe_pos_slice_start = 23 + # left_toe_pos_slice_end = 26 + + # right_toe_pos_slice_start = 26 + # right_toe_pos_slice_end = 29 + + foot_contacts_slice_start = 32 + foot_contacts_slice_end = 34 + + # ref_base_pos = reference_frame[root_pos_slice_start:root_pos_slice_end] + # base_pos = qpos[:3] + + ref_base_orientation_quat = reference_frame[ + root_quat_slice_start:root_quat_slice_end + ] + ref_base_orientation_quat = ref_base_orientation_quat / np.linalg.norm( + ref_base_orientation_quat + ) # normalize the quat + base_orientation = base_qpos[3:7] + base_orientation = base_orientation / np.linalg.norm( + base_orientation + ) # normalize the quat + + ref_base_lin_vel = reference_frame[linear_vel_slice_start:linear_vel_slice_end] + base_lin_vel = base_qvel[:3] + + ref_base_ang_vel = reference_frame[angular_vel_slice_start:angular_vel_slice_end] + base_ang_vel = base_qvel[3:6] + + ref_joint_pos = reference_frame[joint_pos_slice_start:joint_pos_slice_end] + # remove neck head and antennas + ref_joint_pos = np.concatenate([ref_joint_pos[:5], ref_joint_pos[11:]]) + # joint_pos = joints_qpos + joint_pos = np.concatenate([joints_qpos[:5], joints_qpos[9:]]) + + ref_joint_vels = reference_frame[joint_vels_slice_start:joint_vels_slice_end] + # remove neck head and antennas + ref_joint_vels = np.concatenate([ref_joint_vels[:5], ref_joint_vels[11:]]) + # joint_vel = joints_qvel + joint_vel = np.concatenate([joints_qvel[:5], joints_qvel[9:]]) + + # ref_left_toe_pos = reference_frame[left_toe_pos_slice_start:left_toe_pos_slice_end] + # ref_right_toe_pos = reference_frame[right_toe_pos_slice_start:right_toe_pos_slice_end] + + ref_foot_contacts = reference_frame[ + foot_contacts_slice_start:foot_contacts_slice_end + ] + + # reward + # torso_pos_rew = np.exp(-200.0 * np.sum(np.square(base_pos[:2] - ref_base_pos[:2]))) * w_torso_pos + + # real quaternion angle doesn't have the expected effect, switching back for now + # torso_orientation_rew = np.exp(-20 * self.quaternion_angle(base_orientation, ref_base_orientation_quat)) * w_torso_orientation + + # TODO ignore yaw here, we just want xy orientation + torso_orientation_rew = ( + np.exp(-20.0 * np.sum(np.square(base_orientation - ref_base_orientation_quat))) + * w_torso_orientation + ) + + lin_vel_xy_rew = ( + np.exp(-8.0 * np.sum(np.square(base_lin_vel[:2] - ref_base_lin_vel[:2]))) + * w_lin_vel_xy + ) + lin_vel_z_rew = ( + np.exp(-8.0 * np.sum(np.square(base_lin_vel[2] - ref_base_lin_vel[2]))) + * w_lin_vel_z + ) + + ang_vel_xy_rew = ( + np.exp(-2.0 * np.sum(np.square(base_ang_vel[:2] - ref_base_ang_vel[:2]))) + * w_ang_vel_xy + ) + ang_vel_z_rew = ( + np.exp(-2.0 * np.sum(np.square(base_ang_vel[2] - ref_base_ang_vel[2]))) + * w_ang_vel_z + ) + + joint_pos_rew = -np.sum(np.square(joint_pos - ref_joint_pos)) * w_joint_pos + joint_vel_rew = -np.sum(np.square(joint_vel - ref_joint_vels)) * w_joint_vel + + ref_foot_contacts = np.where( + np.array(ref_foot_contacts) > 0.5, + np.ones_like(ref_foot_contacts), + np.zeros_like(ref_foot_contacts), + ) + contact_rew = np.sum(contacts == ref_foot_contacts) * w_contact + + reward = ( + lin_vel_xy_rew + + lin_vel_z_rew + + ang_vel_xy_rew + + ang_vel_z_rew + + joint_pos_rew + + joint_vel_rew + + contact_rew + # + torso_orientation_rew + ) + + reward *= cmd_norm > 0.01 # No reward for zero commands. + return np.nan_to_num(reward) diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/data/polynomial_coefficients.pkl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/data/polynomial_coefficients.pkl new file mode 100644 index 0000000..ebfef40 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/data/polynomial_coefficients.pkl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/joystick.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/joystick.py new file mode 100644 index 0000000..c8d5e7d --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/joystick.py @@ -0,0 +1,725 @@ +# Copyright 2025 DeepMind Technologies Limited +# Copyright 2025 Antoine Pirrone - Steve Nguyen +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Joystick task for Open Duck Mini V2. (based on Berkeley Humanoid)""" + +from typing import Any, Dict, Optional, Union +import jax +import jax.numpy as jp +from ml_collections import config_dict +from mujoco import mjx +from mujoco.mjx._src import math +import numpy as np + +from mujoco_playground._src import mjx_env +from mujoco_playground._src.collision import geoms_colliding + +from . import constants +from . import base as open_duck_mini_v2_base + +# from playground.common.utils import LowPassActionFilter +from playground.common.poly_reference_motion import PolyReferenceMotion +from playground.common.rewards import ( + reward_tracking_lin_vel, + reward_tracking_ang_vel, + cost_torques, + cost_action_rate, + cost_stand_still, + reward_alive, +) +from playground.open_duck_mini_v2.custom_rewards import reward_imitation + +# if set to false, won't require the reference data to be present and won't compute the reference motions polynoms for nothing +USE_IMITATION_REWARD = True +USE_MOTOR_SPEED_LIMITS = True + + +def default_config() -> config_dict.ConfigDict: + return config_dict.create( + ctrl_dt=0.02, + sim_dt=0.002, + episode_length=1000, + action_repeat=1, + action_scale=0.25, + dof_vel_scale=0.05, + history_len=0, + soft_joint_pos_limit_factor=0.95, + max_motor_velocity=5.24, # rad/s + noise_config=config_dict.create( + level=1.0, # Set to 0.0 to disable noise. + action_min_delay=0, # env steps + action_max_delay=3, # env steps + imu_min_delay=0, # env steps + imu_max_delay=3, # env steps + scales=config_dict.create( + hip_pos=0.03, # rad, for each hip joint + knee_pos=0.05, # rad, for each knee joint + ankle_pos=0.08, # rad, for each ankle joint + joint_vel=2.5, # rad/s # Was 1.5 + gravity=0.1, + linvel=0.1, + gyro=0.1, + accelerometer=0.05, + ), + ), + reward_config=config_dict.create( + scales=config_dict.create( + tracking_lin_vel=2.5, + tracking_ang_vel=6.0, + torques=-1.0e-3, + action_rate=-0.5, # was -1.5 + stand_still=-0.2, # was -1.0 TODO try to relax this a bit ? + alive=20.0, + imitation=1.0, + ), + tracking_sigma=0.01, # was working at 0.01 + ), + push_config=config_dict.create( + enable=True, + interval_range=[5.0, 10.0], + magnitude_range=[0.1, 1.0], + ), + lin_vel_x=[-0.15, 0.15], + lin_vel_y=[-0.2, 0.2], + ang_vel_yaw=[-1.0, 1.0], # [-1.0, 1.0] + neck_pitch_range=[-0.34, 1.1], + head_pitch_range=[-0.78, 0.78], + head_yaw_range=[-1.5, 1.5], + head_roll_range=[-0.5, 0.5], + head_range_factor=1.0, # to make it easier + ) + + +class Joystick(open_duck_mini_v2_base.OpenDuckMiniV2Env): + """Track a joystick command.""" + + def __init__( + self, + task: str = "flat_terrain", + config: config_dict.ConfigDict = default_config(), + config_overrides: Optional[Dict[str, Union[str, int, list[Any]]]] = None, + ): + super().__init__( + xml_path=constants.task_to_xml(task).as_posix(), + config=config, + config_overrides=config_overrides, + ) + self._post_init() + + def _post_init(self) -> None: + + self._init_q = jp.array(self._mj_model.keyframe("home").qpos) + self._default_actuator = self._mj_model.keyframe( + "home" + ).ctrl # ctrl of all the actual joints (no floating base and no backlash) + + if USE_IMITATION_REWARD: + self.PRM = PolyReferenceMotion( + "playground/open_duck_mini_v2/data/polynomial_coefficients.pkl" + ) + + # Note: First joint is freejoint. + # get the range of the joints + self._lowers, self._uppers = self.mj_model.jnt_range[1:].T + c = (self._lowers + self._uppers) / 2 + r = self._uppers - self._lowers + self._soft_lowers = c - 0.5 * r * self._config.soft_joint_pos_limit_factor + self._soft_uppers = c + 0.5 * r * self._config.soft_joint_pos_limit_factor + + # weights for computing the cost of each joints compared to a reference pose + self._weights = jp.array( + [ + 1.0, + 1.0, + 0.01, + 0.01, + 1.0, # left leg. + # 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, #head + 1.0, + 1.0, + 0.01, + 0.01, + 1.0, # right leg. + ] + ) + + self._njoints = self._mj_model.njnt # number of joints + self._actuators = self._mj_model.nu # number of actuators + + self._torso_body_id = self._mj_model.body(constants.ROOT_BODY).id + self._torso_mass = self._mj_model.body_subtreemass[self._torso_body_id] + self._site_id = self._mj_model.site("imu").id + + self._feet_site_id = np.array( + [self._mj_model.site(name).id for name in constants.FEET_SITES] + ) + self._floor_geom_id = self._mj_model.geom("floor").id + self._feet_geom_id = np.array( + [self._mj_model.geom(name).id for name in constants.FEET_GEOMS] + ) + + foot_linvel_sensor_adr = [] + for site in constants.FEET_SITES: + sensor_id = self._mj_model.sensor(f"{site}_global_linvel").id + sensor_adr = self._mj_model.sensor_adr[sensor_id] + sensor_dim = self._mj_model.sensor_dim[sensor_id] + foot_linvel_sensor_adr.append( + list(range(sensor_adr, sensor_adr + sensor_dim)) + ) + self._foot_linvel_sensor_adr = jp.array(foot_linvel_sensor_adr) + + # # noise in the simu? + qpos_noise_scale = np.zeros(self._actuators) + + hip_ids = [ + idx for idx, j in enumerate(constants.JOINTS_ORDER_NO_HEAD) if "_hip" in j + ] + knee_ids = [ + idx for idx, j in enumerate(constants.JOINTS_ORDER_NO_HEAD) if "_knee" in j + ] + ankle_ids = [ + idx for idx, j in enumerate(constants.JOINTS_ORDER_NO_HEAD) if "_ankle" in j + ] + + qpos_noise_scale[hip_ids] = self._config.noise_config.scales.hip_pos + qpos_noise_scale[knee_ids] = self._config.noise_config.scales.knee_pos + qpos_noise_scale[ankle_ids] = self._config.noise_config.scales.ankle_pos + # qpos_noise_scale[faa_ids] = self._config.noise_config.scales.faa_pos + self._qpos_noise_scale = jp.array(qpos_noise_scale) + + # self.action_filter = LowPassActionFilter( + # 1 / self._config.ctrl_dt, cutoff_frequency=37.5 + # ) + + def reset(self, rng: jax.Array) -> mjx_env.State: + qpos = self._init_q # the complete qpos + # print(f'DEBUG0 init qpos: {qpos}') + qvel = jp.zeros(self.mjx_model.nv) + + # init position/orientation in environment + # x=+U(-0.05, 0.05), y=+U(-0.05, 0.05), yaw=U(-3.14, 3.14). + rng, key = jax.random.split(rng) + dxy = jax.random.uniform(key, (2,), minval=-0.05, maxval=0.05) + + # floating base + base_qpos = self.get_floating_base_qpos(qpos) + base_qpos = base_qpos.at[0:2].set( + qpos[self._floating_base_qpos_addr : self._floating_base_qpos_addr + 2] + + dxy + ) # x y noise + + rng, key = jax.random.split(rng) + yaw = jax.random.uniform(key, (1,), minval=-3.14, maxval=3.14) + quat = math.axis_angle_to_quat(jp.array([0, 0, 1]), yaw) + new_quat = math.quat_mul( + qpos[self._floating_base_qpos_addr + 3 : self._floating_base_qpos_addr + 7], + quat, + ) # yaw noise + + base_qpos = base_qpos.at[3:7].set(new_quat) + + qpos = self.set_floating_base_qpos(base_qpos, qpos) + # print(f'DEBUG1 base qpos: {qpos}') + # init joint position + # qpos[7:]=*U(0.0, 0.1) + rng, key = jax.random.split(rng) + + # multiply actual joints with noise (excluding floating base and backlash) + qpos_j = self.get_actuator_joints_qpos(qpos) * jax.random.uniform( + key, (self._actuators,), minval=0.5, maxval=1.5 + ) + qpos = self.set_actuator_joints_qpos(qpos_j, qpos) + # print(f'DEBUG2 joint qpos: {qpos}') + # init joint vel + # d(xyzrpy)=U(-0.05, 0.05) + rng, key = jax.random.split(rng) + # qvel = qvel.at[self._floating_base_qvel_addr : self._floating_base_qvel_addr + 6].set( + # jax.random.uniform(key, (6,), minval=-0.5, maxval=0.5) + # ) + + qvel = self.set_floating_base_qvel( + jax.random.uniform(key, (6,), minval=-0.05, maxval=0.05), qvel + ) + # print(f'DEBUG3 base qvel: {qvel}') + ctrl = self.get_actuator_joints_qpos(qpos) + # print(f'DEBUG4 ctrl: {ctrl}') + data = mjx_env.init(self.mjx_model, qpos=qpos, qvel=qvel, ctrl=ctrl) + rng, cmd_rng = jax.random.split(rng) + cmd = self.sample_command(cmd_rng) + + # Sample push interval. + rng, push_rng = jax.random.split(rng) + push_interval = jax.random.uniform( + push_rng, + minval=self._config.push_config.interval_range[0], + maxval=self._config.push_config.interval_range[1], + ) + push_interval_steps = jp.round(push_interval / self.dt).astype(jp.int32) + + if USE_IMITATION_REWARD: + current_reference_motion = self.PRM.get_reference_motion( + cmd[0], cmd[1], cmd[2], 0 + ) + else: + current_reference_motion = jp.zeros(0) + + info = { + "rng": rng, + "step": 0, + "command": cmd, + "last_act": jp.zeros(self.mjx_model.nu), + "last_last_act": jp.zeros(self.mjx_model.nu), + "last_last_last_act": jp.zeros(self.mjx_model.nu), + "motor_targets": self._default_actuator, + "feet_air_time": jp.zeros(2), + "last_contact": jp.zeros(2, dtype=bool), + "swing_peak": jp.zeros(2), + # Push related. + "push": jp.array([0.0, 0.0]), + "push_step": 0, + "push_interval_steps": push_interval_steps, + # History related. + "action_history": jp.zeros( + self._config.noise_config.action_max_delay * self._actuators + ), + "imu_history": jp.zeros(self._config.noise_config.imu_max_delay * 3), + # imitation related + "imitation_i": 0, + "current_reference_motion": current_reference_motion, + "imitation_phase": jp.zeros(2), + } + + metrics = {} + for k, v in self._config.reward_config.scales.items(): + if v != 0: + if v > 0: + metrics[f"reward/{k}"] = jp.zeros(()) + else: + metrics[f"cost/{k}"] = jp.zeros(()) + metrics["swing_peak"] = jp.zeros(()) + + contact = jp.array( + [ + geoms_colliding(data, geom_id, self._floor_geom_id) + for geom_id in self._feet_geom_id + ] + ) + obs = self._get_obs(data, info, contact) + reward, done = jp.zeros(2) + return mjx_env.State(data, obs, reward, done, metrics, info) + + def step(self, state: mjx_env.State, action: jax.Array) -> mjx_env.State: + + if USE_IMITATION_REWARD: + state.info["imitation_i"] += 1 + state.info["imitation_i"] = ( + state.info["imitation_i"] % self.PRM.nb_steps_in_period + ) # not critical, is already moduloed in get_reference_motion + state.info["imitation_phase"] = jp.array( + [ + jp.cos( + (state.info["imitation_i"] / self.PRM.nb_steps_in_period) + * 2 + * jp.pi + ), + jp.sin( + (state.info["imitation_i"] / self.PRM.nb_steps_in_period) + * 2 + * jp.pi + ), + ] + ) + else: + state.info["imitation_i"] = 0 + + if USE_IMITATION_REWARD: + state.info["current_reference_motion"] = self.PRM.get_reference_motion( + state.info["command"][0], + state.info["command"][1], + state.info["command"][2], + state.info["imitation_i"], + ) + else: + state.info["current_reference_motion"] = jp.zeros(0) + + state.info["rng"], push1_rng, push2_rng, action_delay_rng = jax.random.split( + state.info["rng"], 4 + ) + + # Handle action delay + action_history = ( + jp.roll(state.info["action_history"], self._actuators) + .at[: self._actuators] + .set(action) + ) + state.info["action_history"] = action_history + action_idx = jax.random.randint( + action_delay_rng, + (1,), + minval=self._config.noise_config.action_min_delay, + maxval=self._config.noise_config.action_max_delay, + ) + action_w_delay = action_history.reshape((-1, self._actuators))[ + action_idx[0] + ] # action with delay + + # self.action_filter.push(action_w_delay) + # action_w_delay = self.action_filter.get_filtered_action() + + push_theta = jax.random.uniform(push1_rng, maxval=2 * jp.pi) + push_magnitude = jax.random.uniform( + push2_rng, + minval=self._config.push_config.magnitude_range[0], + maxval=self._config.push_config.magnitude_range[1], + ) + push = jp.array([jp.cos(push_theta), jp.sin(push_theta)]) + push *= ( + jp.mod(state.info["push_step"] + 1, state.info["push_interval_steps"]) == 0 + ) + push *= self._config.push_config.enable + qvel = state.data.qvel + qvel = qvel.at[ + self._floating_base_qvel_addr : self._floating_base_qvel_addr + 2 + ].set( + push * push_magnitude + + qvel[self._floating_base_qvel_addr : self._floating_base_qvel_addr + 2] + ) # floating base x,y + data = state.data.replace(qvel=qvel) + state = state.replace(data=data) + + #### + + motor_targets = ( + self._default_actuator + action_w_delay * self._config.action_scale + ) + + if USE_MOTOR_SPEED_LIMITS: + prev_motor_targets = state.info["motor_targets"] + + motor_targets = jp.clip( + motor_targets, + prev_motor_targets + - self._config.max_motor_velocity * self.dt, # control dt + prev_motor_targets + + self._config.max_motor_velocity * self.dt, # control dt + ) + + # motor_targets.at[5:9].set(state.info["command"][3:]) # head joints + data = mjx_env.step(self.mjx_model, state.data, motor_targets, self.n_substeps) + + state.info["motor_targets"] = motor_targets + + contact = jp.array( + [ + geoms_colliding(data, geom_id, self._floor_geom_id) + for geom_id in self._feet_geom_id + ] + ) + contact_filt = contact | state.info["last_contact"] + first_contact = (state.info["feet_air_time"] > 0.0) * contact_filt + state.info["feet_air_time"] += self.dt + p_f = data.site_xpos[self._feet_site_id] + p_fz = p_f[..., -1] + state.info["swing_peak"] = jp.maximum(state.info["swing_peak"], p_fz) + + obs = self._get_obs(data, state.info, contact) + done = self._get_termination(data) + + rewards = self._get_reward( + data, action, state.info, state.metrics, done, first_contact, contact + ) + # FIXME + rewards = { + k: v * self._config.reward_config.scales[k] for k, v in rewards.items() + } + reward = jp.clip(sum(rewards.values()) * self.dt, 0.0, 10000.0) + # jax.debug.print('STEP REWARD: {}',reward) + state.info["push"] = push + state.info["step"] += 1 + state.info["push_step"] += 1 + state.info["last_last_last_act"] = state.info["last_last_act"] + state.info["last_last_act"] = state.info["last_act"] + state.info["last_act"] = action # was + # state.info["last_act"] = motor_targets # became + state.info["rng"], cmd_rng = jax.random.split(state.info["rng"]) + state.info["command"] = jp.where( + state.info["step"] > 500, + self.sample_command(cmd_rng), + state.info["command"], + ) + state.info["step"] = jp.where( + done | (state.info["step"] > 500), + 0, + state.info["step"], + ) + state.info["feet_air_time"] *= ~contact + state.info["last_contact"] = contact + state.info["swing_peak"] *= ~contact + for k, v in rewards.items(): + rew_scale = self._config.reward_config.scales[k] + if rew_scale != 0: + if rew_scale > 0: + state.metrics[f"reward/{k}"] = v + else: + state.metrics[f"cost/{k}"] = -v + state.metrics["swing_peak"] = jp.mean(state.info["swing_peak"]) + + done = done.astype(reward.dtype) + state = state.replace(data=data, obs=obs, reward=reward, done=done) + return state + + def _get_termination(self, data: mjx.Data) -> jax.Array: + fall_termination = self.get_gravity(data)[-1] < 0.0 + return fall_termination | jp.isnan(data.qpos).any() | jp.isnan(data.qvel).any() + + def _get_obs( + self, data: mjx.Data, info: dict[str, Any], contact: jax.Array + ) -> mjx_env.Observation: + + gyro = self.get_gyro(data) + info["rng"], noise_rng = jax.random.split(info["rng"]) + noisy_gyro = ( + gyro + + (2 * jax.random.uniform(noise_rng, shape=gyro.shape) - 1) + * self._config.noise_config.level + * self._config.noise_config.scales.gyro + ) + + accelerometer = self.get_accelerometer(data) + # accelerometer[0] += 1.3 # TODO testing + accelerometer.at[0].set(accelerometer[0] + 1.3) + + info["rng"], noise_rng = jax.random.split(info["rng"]) + noisy_accelerometer = ( + accelerometer + + (2 * jax.random.uniform(noise_rng, shape=accelerometer.shape) - 1) + * self._config.noise_config.level + * self._config.noise_config.scales.accelerometer + ) + + gravity = data.site_xmat[self._site_id].T @ jp.array([0, 0, -1]) + info["rng"], noise_rng = jax.random.split(info["rng"]) + noisy_gravity = ( + gravity + + (2 * jax.random.uniform(noise_rng, shape=gravity.shape) - 1) + * self._config.noise_config.level + * self._config.noise_config.scales.gravity + ) + + # Handle IMU delay + imu_history = jp.roll(info["imu_history"], 3).at[:3].set(noisy_gravity) + info["imu_history"] = imu_history + imu_idx = jax.random.randint( + noise_rng, + (1,), + minval=self._config.noise_config.imu_min_delay, + maxval=self._config.noise_config.imu_max_delay, + ) + noisy_gravity = imu_history.reshape((-1, 3))[imu_idx[0]] + + # joint_angles = data.qpos[7:] + + # Handling backlash + joint_angles = self.get_actuator_joints_qpos(data.qpos) + joint_backlash = self.get_actuator_backlash_qpos(data.qpos) + + for i in self.backlash_idx_to_add: + joint_backlash = jp.insert(joint_backlash, i, 0) + + joint_angles = joint_angles + joint_backlash + + info["rng"], noise_rng = jax.random.split(info["rng"]) + noisy_joint_angles = ( + joint_angles + + (2.0 * jax.random.uniform(noise_rng, shape=joint_angles.shape) - 1.0) + * self._config.noise_config.level + * self._qpos_noise_scale + ) + + # joint_vel = data.qvel[6:] + joint_vel = self.get_actuator_joints_qvel(data.qvel) + info["rng"], noise_rng = jax.random.split(info["rng"]) + noisy_joint_vel = ( + joint_vel + + (2.0 * jax.random.uniform(noise_rng, shape=joint_vel.shape) - 1.0) + * self._config.noise_config.level + * self._config.noise_config.scales.joint_vel + ) + + linvel = self.get_local_linvel(data) + # info["rng"], noise_rng = jax.random.split(info["rng"]) + # noisy_linvel = ( + # linvel + # + (2 * jax.random.uniform(noise_rng, shape=linvel.shape) - 1) + # * self._config.noise_config.level + # * self._config.noise_config.scales.linvel + # ) + + state = jp.hstack( + [ + # noisy_linvel, # 3 + # noisy_gyro, # 3 + # noisy_gravity, # 3 + noisy_gyro, # 3 + noisy_accelerometer, # 3 + info["command"], # 3 + noisy_joint_angles - self._default_actuator, # 10 + noisy_joint_vel * self._config.dof_vel_scale, # 10 + info["last_act"], # 10 + info["last_last_act"], # 10 + info["last_last_last_act"], # 10 + info["motor_targets"], # 10 + contact, # 2 + # info["current_reference_motion"], + # info["imitation_i"], + info["imitation_phase"], + ] + ) + + accelerometer = self.get_accelerometer(data) + global_angvel = self.get_global_angvel(data) + feet_vel = data.sensordata[self._foot_linvel_sensor_adr].ravel() + root_height = data.qpos[self._floating_base_qpos_addr + 2] + + privileged_state = jp.hstack( + [ + state, + gyro, # 3 + accelerometer, # 3 + gravity, # 3 + linvel, # 3 + global_angvel, # 3 + joint_angles - self._default_actuator, + joint_vel, + root_height, # 1 + data.actuator_force, # 10 + contact, # 2 + feet_vel, # 4*3 + info["feet_air_time"], # 2 + info["current_reference_motion"], + info["imitation_i"], + info["imitation_phase"], + ] + ) + + return { + "state": state, + "privileged_state": privileged_state, + } + + def _get_reward( + self, + data: mjx.Data, + action: jax.Array, + info: dict[str, Any], + metrics: dict[str, Any], + done: jax.Array, + first_contact: jax.Array, + contact: jax.Array, + ) -> dict[str, jax.Array]: + del metrics # Unused. + + ret = { + "tracking_lin_vel": reward_tracking_lin_vel( + info["command"], + self.get_local_linvel(data), + self._config.reward_config.tracking_sigma, + ), + "tracking_ang_vel": reward_tracking_ang_vel( + info["command"], + self.get_gyro(data), + self._config.reward_config.tracking_sigma, + ), + # "orientation": cost_orientation(self.get_gravity(data)), + "torques": cost_torques(data.actuator_force), + "action_rate": cost_action_rate(action, info["last_act"]), + "alive": reward_alive(), + "imitation": reward_imitation( # FIXME, this reward is so adhoc... + self.get_floating_base_qpos(data.qpos), # floating base qpos + self.get_floating_base_qvel(data.qvel), # floating base qvel + self.get_actuator_joints_qpos(data.qpos), + self.get_actuator_joints_qvel(data.qvel), + contact, + info["current_reference_motion"], + info["command"], + USE_IMITATION_REWARD, + ), + "stand_still": cost_stand_still( + # info["command"], data.qpos[7:], data.qvel[6:], self._default_pose + info["command"], + self.get_actuator_joints_qpos(data.qpos), + self.get_actuator_joints_qvel(data.qvel), + self._default_actuator, + ignore_head=False, + ), + } + + return ret + + def sample_command(self, rng: jax.Array) -> jax.Array: + rng1, rng2, rng3, rng4, rng5, rng6, rng7, rng8 = jax.random.split(rng, 8) + + lin_vel_x = jax.random.uniform( + rng1, minval=self._config.lin_vel_x[0], maxval=self._config.lin_vel_x[1] + ) + lin_vel_y = jax.random.uniform( + rng2, minval=self._config.lin_vel_y[0], maxval=self._config.lin_vel_y[1] + ) + ang_vel_yaw = jax.random.uniform( + rng3, + minval=self._config.ang_vel_yaw[0], + maxval=self._config.ang_vel_yaw[1], + ) + + neck_pitch = jax.random.uniform( + rng5, + minval=self._config.neck_pitch_range[0] * self._config.head_range_factor, + maxval=self._config.neck_pitch_range[1] * self._config.head_range_factor, + ) + + head_pitch = jax.random.uniform( + rng6, + minval=self._config.head_pitch_range[0] * self._config.head_range_factor, + maxval=self._config.head_pitch_range[1] * self._config.head_range_factor, + ) + + head_yaw = jax.random.uniform( + rng7, + minval=self._config.head_yaw_range[0] * self._config.head_range_factor, + maxval=self._config.head_yaw_range[1] * self._config.head_range_factor, + ) + + head_roll = jax.random.uniform( + rng8, + minval=self._config.head_roll_range[0] * self._config.head_range_factor, + maxval=self._config.head_roll_range[1] * self._config.head_range_factor, + ) + + # With 10% chance, set everything to zero. + return jp.where( + jax.random.bernoulli(rng4, p=0.1), + jp.zeros(7), + jp.hstack( + [ + lin_vel_x, + lin_vel_y, + ang_vel_yaw, + neck_pitch, + head_pitch, + head_yaw, + head_roll, + ] + ), + ) diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/mujoco_infer.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/mujoco_infer.py new file mode 100644 index 0000000..8b99164 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/mujoco_infer.py @@ -0,0 +1,266 @@ +import mujoco +import pickle +import numpy as np +import mujoco +import mujoco.viewer +import time +import argparse +from playground.common.onnx_infer import OnnxInfer +from playground.common.poly_reference_motion_numpy import PolyReferenceMotion +from playground.common.utils import LowPassActionFilter + +from playground.open_duck_mini_v2.mujoco_infer_base import MJInferBase + +USE_MOTOR_SPEED_LIMITS = True + + +class MjInfer(MJInferBase): + def __init__( + self, model_path: str, reference_data: str, onnx_model_path: str, standing: bool + ): + super().__init__(model_path) + + self.standing = standing + self.head_control_mode = self.standing + + # Params + self.linearVelocityScale = 1.0 + self.angularVelocityScale = 1.0 + self.dof_pos_scale = 1.0 + self.dof_vel_scale = 0.05 + self.action_scale = 0.25 + + self.action_filter = LowPassActionFilter(50, cutoff_frequency=37.5) + + if not self.standing: + self.PRM = PolyReferenceMotion(reference_data) + + self.policy = OnnxInfer(onnx_model_path, awd=True) + + self.COMMANDS_RANGE_X = [-0.15, 0.15] + self.COMMANDS_RANGE_Y = [-0.2, 0.2] + self.COMMANDS_RANGE_THETA = [-1.0, 1.0] # [-1.0, 1.0] + + self.NECK_PITCH_RANGE = [-0.34, 1.1] + self.HEAD_PITCH_RANGE = [-0.78, 0.78] + self.HEAD_YAW_RANGE = [-1.5, 1.5] + self.HEAD_ROLL_RANGE = [-0.5, 0.5] + + self.last_action = np.zeros(self.num_dofs) + self.last_last_action = np.zeros(self.num_dofs) + self.last_last_last_action = np.zeros(self.num_dofs) + self.commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + self.imitation_i = 0 + self.imitation_phase = np.array([0, 0]) + self.saved_obs = [] + + self.max_motor_velocity = 5.24 # rad/s + + self.phase_frequency_factor = 1.0 + + print(f"joint names: {self.joint_names}") + print(f"actuator names: {self.actuator_names}") + print(f"backlash joint names: {self.backlash_joint_names}") + # print(f"actual joints idx: {self.get_actual_joints_idx()}") + + def get_obs( + self, + data, + command, # , qvel_history, qpos_error_history, gravity_history + ): + gyro = self.get_gyro(data) + accelerometer = self.get_accelerometer(data) + accelerometer[0] += 1.3 + + joint_angles = self.get_actuator_joints_qpos(data.qpos) + joint_vel = self.get_actuator_joints_qvel(data.qvel) + + contacts = self.get_feet_contacts(data) + + # if not self.standing: + # ref = self.PRM.get_reference_motion(*command[:3], self.imitation_i) + + obs = np.concatenate( + [ + gyro, + accelerometer, + # gravity, + command, + joint_angles - self.default_actuator, + joint_vel * self.dof_vel_scale, + self.last_action, + self.last_last_action, + self.last_last_last_action, + self.motor_targets, + contacts, + # ref if not self.standing else np.array([]), + # [self.imitation_i] + self.imitation_phase, + ] + ) + + return obs + + def key_callback(self, keycode): + print(f"key: {keycode}") + if keycode == 72: # h + self.head_control_mode = not self.head_control_mode + lin_vel_x = 0 + lin_vel_y = 0 + ang_vel = 0 + if not self.head_control_mode: + if keycode == 265: # arrow up + lin_vel_x = self.COMMANDS_RANGE_X[1] + if keycode == 264: # arrow down + lin_vel_x = self.COMMANDS_RANGE_X[0] + if keycode == 263: # arrow left + lin_vel_y = self.COMMANDS_RANGE_Y[1] + if keycode == 262: # arrow right + lin_vel_y = self.COMMANDS_RANGE_Y[0] + if keycode == 81: # a + ang_vel = self.COMMANDS_RANGE_THETA[1] + if keycode == 69: # e + ang_vel = self.COMMANDS_RANGE_THETA[0] + if keycode == 80: # p + self.phase_frequency_factor += 0.1 + if keycode == 59: # m + self.phase_frequency_factor -= 0.1 + else: + neck_pitch = 0 + head_pitch = 0 + head_yaw = 0 + head_roll = 0 + if keycode == 265: # arrow up + head_pitch = self.NECK_PITCH_RANGE[1] + if keycode == 264: # arrow down + head_pitch = self.NECK_PITCH_RANGE[0] + if keycode == 263: # arrow left + head_yaw = self.HEAD_YAW_RANGE[1] + if keycode == 262: # arrow right + head_yaw = self.HEAD_YAW_RANGE[0] + if keycode == 81: # a + head_roll = self.HEAD_ROLL_RANGE[1] + if keycode == 69: # e + head_roll = self.HEAD_ROLL_RANGE[0] + + self.commands[3] = neck_pitch + self.commands[4] = head_pitch + self.commands[5] = head_yaw + self.commands[6] = head_roll + + self.commands[0] = lin_vel_x + self.commands[1] = lin_vel_y + self.commands[2] = ang_vel + + def run(self): + try: + with mujoco.viewer.launch_passive( + self.model, + self.data, + show_left_ui=False, + show_right_ui=False, + key_callback=self.key_callback, + ) as viewer: + counter = 0 + while True: + + step_start = time.time() + + mujoco.mj_step(self.model, self.data) + + counter += 1 + + if counter % self.decimation == 0: + if not self.standing: + self.imitation_i += 1.0 * self.phase_frequency_factor + self.imitation_i = ( + self.imitation_i % self.PRM.nb_steps_in_period + ) + # print(self.PRM.nb_steps_in_period) + # exit() + self.imitation_phase = np.array( + [ + np.cos( + self.imitation_i + / self.PRM.nb_steps_in_period + * 2 + * np.pi + ), + np.sin( + self.imitation_i + / self.PRM.nb_steps_in_period + * 2 + * np.pi + ), + ] + ) + obs = self.get_obs( + self.data, + self.commands, + ) + self.saved_obs.append(obs) + action = self.policy.infer(obs) + + # self.action_filter.push(action) + # action = self.action_filter.get_filtered_action() + + self.last_last_last_action = self.last_last_action.copy() + self.last_last_action = self.last_action.copy() + self.last_action = action.copy() + + self.motor_targets = ( + self.default_actuator + action * self.action_scale + ) + + if USE_MOTOR_SPEED_LIMITS: + self.motor_targets = np.clip( + self.motor_targets, + self.prev_motor_targets + - self.max_motor_velocity + * (self.sim_dt * self.decimation), + self.prev_motor_targets + + self.max_motor_velocity + * (self.sim_dt * self.decimation), + ) + + self.prev_motor_targets = self.motor_targets.copy() + + # head_targets = self.commands[3:] + # self.motor_targets[5:9] = head_targets + self.data.ctrl = self.motor_targets.copy() + + viewer.sync() + + time_until_next_step = self.model.opt.timestep - ( + time.time() - step_start + ) + if time_until_next_step > 0: + time.sleep(time_until_next_step) + except KeyboardInterrupt: + pickle.dump(self.saved_obs, open("mujoco_saved_obs.pkl", "wb")) + + +if __name__ == "__main__": + + parser = argparse.ArgumentParser() + parser.add_argument("-o", "--onnx_model_path", type=str, required=True) + # parser.add_argument("-k", action="store_true", default=False) + parser.add_argument( + "--reference_data", + type=str, + default="playground/open_duck_mini_v2/data/polynomial_coefficients.pkl", + ) + parser.add_argument( + "--model_path", + type=str, + default="playground/open_duck_mini_v2/xmls/scene_flat_terrain.xml", + ) + parser.add_argument("--standing", action="store_true", default=False) + + args = parser.parse_args() + + mjinfer = MjInfer( + args.model_path, args.reference_data, args.onnx_model_path, args.standing + ) + mjinfer.run() diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/mujoco_infer_base.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/mujoco_infer_base.py new file mode 100644 index 0000000..5541f6b --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/mujoco_infer_base.py @@ -0,0 +1,283 @@ +import mujoco +import numpy as np +from etils import epath +from playground.open_duck_mini_v2 import base + + +class MJInferBase: + def __init__(self, model_path): + + self.model = mujoco.MjModel.from_xml_string( + epath.Path(model_path).read_text(), assets=base.get_assets() + ) + print(model_path) + + self.sim_dt = 0.002 + self.decimation = 10 + self.model.opt.timestep = self.sim_dt + self.data = mujoco.MjData(self.model) + mujoco.mj_step(self.model, self.data) + + self.num_dofs = self.model.nu + self.floating_base_name = [ + self.model.jnt(k).name + for k in range(0, self.model.njnt) + if self.model.jnt(k).type == 0 + ][ + 0 + ] # assuming only one floating object! + self.actuator_names = [ + self.model.actuator(k).name for k in range(0, self.model.nu) + ] # will be useful to get only the actuators we care about + self.joint_names = [ # njnt = all joints (including floating base, actuators and backlash joints) + self.model.jnt(k).name for k in range(0, self.model.njnt) + ] # all the joint (including the backlash joints) + self.backlash_joint_names = [ + j + for j in self.joint_names + if j not in self.actuator_names and j not in self.floating_base_name + ] # only the dummy backlash joint + self.all_joint_ids = [self.get_joint_id_from_name(n) for n in self.joint_names] + self.all_joint_qpos_addr = [ + self.get_joint_addr_from_name(n) for n in self.joint_names + ] + + self.actuator_joint_ids = [ + self.get_joint_id_from_name(n) for n in self.actuator_names + ] + self.actuator_joint_qpos_addr = [ + self.get_joint_addr_from_name(n) for n in self.actuator_names + ] + + self.backlash_joint_ids = [ + self.get_joint_id_from_name(n) for n in self.backlash_joint_names + ] + + self.backlash_joint_qpos_addr = [ + self.get_joint_addr_from_name(n) for n in self.backlash_joint_names + ] + + self.all_qvel_addr = np.array( + [self.model.jnt_dofadr[jad] for jad in self.all_joint_ids] + ) + self.actuator_qvel_addr = np.array( + [self.model.jnt_dofadr[jad] for jad in self.actuator_joint_ids] + ) + + self.actuator_joint_dict = { + n: self.get_joint_id_from_name(n) for n in self.actuator_names + } + + self._floating_base_qpos_addr = self.model.jnt_qposadr[ + np.where(self.model.jnt_type == 0) + ][ + 0 + ] # Assuming there is only one floating base! the jnt_type==0 is a floating joint. 3 is a hinge + + self._floating_base_qvel_addr = self.model.jnt_dofadr[ + np.where(self.model.jnt_type == 0) + ][ + 0 + ] # Assuming there is only one floating base! the jnt_type==0 is a floating joint. 3 is a hinge + + self._floating_base_id = self.model.joint(self.floating_base_name).id + + # self.all_joint_no_backlash_ids=np.zeros(7+self.model.nu) + all_idx = self.backlash_joint_ids + list( + range(self._floating_base_qpos_addr, self._floating_base_qpos_addr + 7) + ) + all_idx.sort() + + # self.all_joint_no_backlash_ids=[idx for idx in self.all_joint_ids if idx not in self.backlash_joint_ids]+list(range(self._floating_base_add,self._floating_base_add+7)) + self.all_joint_no_backlash_ids = [idx for idx in all_idx] + + self.gyro_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SENSOR, "gyro") + self.gyro_addr = self.model.sensor_adr[self.gyro_id] + self.gyro_dimensions = 3 + + self.accelerometer_id = mujoco.mj_name2id( + self.model, mujoco.mjtObj.mjOBJ_SENSOR, "accelerometer" + ) + self.accelerometer_dimensions = 3 + self.accelerometer_addr = self.model.sensor_adr[self.accelerometer_id] + + self.linvel_id = mujoco.mj_name2id( + self.model, mujoco.mjtObj.mjOBJ_SENSOR, "local_linvel" + ) + self.linvel_dimensions = 3 + + self.imu_site_id = mujoco.mj_name2id( + self.model, mujoco.mjtObj.mjOBJ_SITE, "imu" + ) + + self.gravity_id = mujoco.mj_name2id( + self.model, mujoco.mjtObj.mjOBJ_SENSOR, "upvector" + ) + self.gravity_dimensions = 3 + + self.init_pos = np.array( + self.get_all_joints_qpos(self.model.keyframe("home").qpos) + ) # pose of all the joints (no floating base) + self.default_actuator = self.model.keyframe( + "home" + ).ctrl # ctrl of all the actual joints (no floating base and no backlash) + self.motor_targets = self.default_actuator + self.prev_motor_targets = self.default_actuator + + self.data.qpos[:] = self.model.keyframe("home").qpos + self.data.ctrl[:] = self.default_actuator + + def get_actuator_id_from_name(self, name: str) -> int: + """Return the id of a specified actuator""" + return mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, name) + + def get_joint_id_from_name(self, name: str) -> int: + """Return the id of a specified joint""" + return mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name) + + def get_joint_addr_from_name(self, name: str) -> int: + """Return the address of a specified joint""" + return self.model.joint(name).qposadr + + def get_dof_id_from_name(self, name: str) -> int: + """Return the id of a specified dof""" + return mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_DOF, name) + + def get_actuator_joint_qpos_from_name( + self, data: np.ndarray, name: str + ) -> np.ndarray: + """Return the qpos of a given actual joint""" + addr = self.model.jnt_qposadr[self.actuator_joint_dict[name]] + return data[addr] + + def get_actuator_joints_addr(self) -> np.ndarray: + """Return the all the idx of actual joints""" + addr = np.array( + [self.model.jnt_qposadr[idx] for idx in self.actuator_joint_ids] + ) + return addr + + def get_floating_base_qpos(self, data: np.ndarray) -> np.ndarray: + return data[self._floating_base_qpos_addr : self._floating_base_qvel_addr + 7] + + def get_floating_base_qvel(self, data: np.ndarray) -> np.ndarray: + return data[self._floating_base_qvel_addr : self._floating_base_qvel_addr + 6] + + def set_floating_base_qpos( + self, new_qpos: np.ndarray, qpos: np.ndarray + ) -> np.ndarray: + qpos[self._floating_base_qpos_addr : self._floating_base_qpos_addr + 7] = ( + new_qpos + ) + return qpos + + def set_floating_base_qvel( + self, new_qvel: np.ndarray, qvel: np.ndarray + ) -> np.ndarray: + qvel[self._floating_base_qvel_addr : self._floating_base_qvel_addr + 6] = ( + new_qvel + ) + return qvel + + def exclude_backlash_joints_addr(self) -> np.ndarray: + """Return the all the idx of actual joints and floating base""" + addr = np.array( + [self.model.jnt_qposadr[idx] for idx in self.all_joint_no_backlash_ids] + ) + return addr + + def get_all_joints_addr(self) -> np.ndarray: + """Return the all the idx of all joints""" + addr = np.array([self.model.jnt_qposadr[idx] for idx in self.all_joint_ids]) + return addr + + def get_actuator_joints_qpos(self, data: np.ndarray) -> np.ndarray: + """Return the all the qpos of actual joints""" + return data[self.get_actuator_joints_addr()] + + def set_actuator_joints_qpos( + self, new_qpos: np.ndarray, qpos: np.ndarray + ) -> np.ndarray: + """Set the qpos only for the actual joints (omit the backlash joint)""" + qpos[self.get_actuator_joints_addr()] = new_qpos + return qpos + + def get_actuator_joints_qvel(self, data: np.ndarray) -> np.ndarray: + """Return the all the qvel of actual joints""" + return data[self.actuator_qvel_addr] + + def set_actuator_joints_qvel( + self, new_qvel: np.ndarray, qvel: np.ndarray + ) -> np.ndarray: + """Set the qvel only for the actual joints (omit the backlash joint)""" + qvel[self.actuator_qvel_addr] = new_qvel + return qvel + + def get_all_joints_qpos(self, data: np.ndarray) -> np.ndarray: + """Return the all the qpos of all joints""" + return data[self.get_all_joints_addr()] + + def get_all_joints_qvel(self, data: np.ndarray) -> np.ndarray: + """Return the all the qvel of all joints""" + return data[self.all_qvel_addr] + + def get_joints_nobacklash_qpos(self, data: np.ndarray) -> np.ndarray: + """Return the all the qpos of actual joints with the floating base""" + return data[self.exclude_backlash_joints_addr()] + + def set_complete_qpos_from_joints( + self, no_backlash_qpos: np.ndarray, full_qpos: np.ndarray + ) -> np.ndarray: + """In the case of backlash joints, we want to ignore them (remove them) but we still need to set the complete state incuding them""" + full_qpos[self.exclude_backlash_joints_addr()] = no_backlash_qpos + return np.array(full_qpos) + + def get_sensor(self, data, name, dimensions): + i = self.model.sensor_name2id(name) + return data.sensordata[i : i + dimensions] + + def get_gyro(self, data): + return data.sensordata[self.gyro_addr : self.gyro_addr + self.gyro_dimensions] + + def get_accelerometer(self, data): + return data.sensordata[ + self.accelerometer_addr : self.accelerometer_addr + + self.accelerometer_dimensions + ] + + def get_linvel(self, data): + return data.sensordata[self.linvel_id : self.linvel_id + self.linvel_dimensions] + + # def get_gravity(self, data): + # return data.site_xmat[self.imu_site_id].reshape((3, 3)).T @ np.array([0, 0, -1]) + + def get_gravity(self, data): + return data.sensordata[ + self.gravity_id : self.gravity_id + self.gravity_dimensions + ] + + def check_contact(self, data, body1_name, body2_name): + body1_id = data.body(body1_name).id + body2_id = data.body(body2_name).id + + for i in range(data.ncon): + try: + contact = data.contact[i] + except Exception as e: + return False + + if ( + self.model.geom_bodyid[contact.geom1] == body1_id + and self.model.geom_bodyid[contact.geom2] == body2_id + ) or ( + self.model.geom_bodyid[contact.geom1] == body2_id + and self.model.geom_bodyid[contact.geom2] == body1_id + ): + return True + + return False + + def get_feet_contacts(self, data): + left_contact = self.check_contact(data, "foot_assembly", "floor") + right_contact = self.check_contact(data, "foot_assembly_2", "floor") + return left_contact, right_contact diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/ref_motion_viewer.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/ref_motion_viewer.py new file mode 100644 index 0000000..f4d4afa --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/ref_motion_viewer.py @@ -0,0 +1,214 @@ +import mujoco +import numpy as np +import time +import argparse +import os, sys +import pygame +from etils import epath +import mujoco.viewer + +# Import the reference motion class. +from playground.open_duck_mini_v2 import base + +from playground.common.poly_reference_motion_numpy import PolyReferenceMotion + +SCRIPT_PATH = os.path.dirname(os.path.abspath(__file__)) +SCENE_PATH = f"{SCRIPT_PATH}/xmls" + +COMMANDS_RANGE_X = [-0.15, 0.15] +COMMANDS_RANGE_Y = [-0.2, 0.2] +COMMANDS_RANGE_THETA = [-1.0, 1.0] # [-1.0, 1.0] + +available_scenes = [] +if os.path.isdir(SCENE_PATH): + for name in os.listdir(SCENE_PATH): + if ( + name.startswith("scene_") + and name.endswith(".xml") + and os.path.isfile(os.path.join(SCENE_PATH, name)) + ): + scene_name = name[len("scene_") : -len(".xml")] + available_scenes.append(scene_name) +if len(available_scenes) == 0: + print(f"No scenes found in: {SCENE_PATH}") + sys.exit(1) + +# Parse command-line arguments. +parser = argparse.ArgumentParser(description="Reference Motion Viewer") +parser.add_argument( + "--reference_data", + type=str, + default="playground/go_bdx/data/polynomial_coefficients.pkl", + help="Path to the polynomial coefficients pickle file.", +) +parser.add_argument( + "-joystick", action="store_true", default=False, help="Use joystick control" +) +# Command parameters: dx, dy, dtheta +parser.add_argument( + "--command", + nargs=3, + type=float, + default=[0.0, -0.05, -0.1], + help="Reference motion command as three floats: dx, dy, dtheta.", +) +parser.add_argument( + "--scene", + type=str, + choices=available_scenes, + default="flat_terrain", +) +args = parser.parse_args() +# model_path = f"playground/go_bdx/xmls/scene_mjx_{args.scene}.xml" +model_path = f"playground/open_duck_mini_v2/xmls/scene_{args.scene}.xml" + +command = args.command + +joystick1 = None +joystick2 = None +if args.joystick: + pygame.init() + pygame.joystick.init() + if pygame.joystick.get_count() > 0: + joystick1 = pygame.joystick.Joystick(0) + joystick1.init() + command = [0.0, 0.0, 0.0] + print("Joystick initialized:", joystick1.get_name()) + if pygame.joystick.get_count() > 1: + joystick2 = pygame.joystick.Joystick(1) + joystick2.init() + print("Joystick 2 (theta) initialized:", joystick2.get_name()) + else: + print( + "Only one joystick detected; theta via second joystick will be disabled." + ) + else: + print("No joystick found!") + +# Load the Mujoco model XML. +model = mujoco.MjModel.from_xml_string( + epath.Path(model_path).read_text(), assets=base.get_assets() +) +data = mujoco.MjData(model) +model.opt.timestep = 0.002 + +# Step the simulation once to initialize. +mujoco.mj_step(model, data) + +# Load the polynomial reference motion. +PRM = PolyReferenceMotion(args.reference_data) + +# Get the "home" keyframe to use as a default pose. +home_frame = model.keyframe("home") +default_qpos = np.array(home_frame.qpos) +default_ctrl = np.array(home_frame.ctrl) +default_qpos[2] += 0.2 # Increase the base height by 0.2 meters + +# Set initial state. +data.qpos[:] = default_qpos.copy() +data.ctrl[:] = default_ctrl.copy() + +decimation = 10 # 50 hz + + +def key_callback(keycode): + if joystick1 is not None: + return + + print(f"key: {keycode}") + lin_vel_x = 0 + lin_vel_y = 0 + ang_vel = 0 + if keycode == 265: # arrow up + lin_vel_x = COMMANDS_RANGE_X[1] + if keycode == 264: # arrow down + lin_vel_x = COMMANDS_RANGE_X[0] + if keycode == 263: # arrow left + lin_vel_y = COMMANDS_RANGE_Y[1] + if keycode == 262: # arrow right + lin_vel_y = COMMANDS_RANGE_Y[0] + if keycode == 81: # a + ang_vel = COMMANDS_RANGE_THETA[1] + if keycode == 69: # e + ang_vel = COMMANDS_RANGE_THETA[0] + + command[0] = lin_vel_x + command[1] = lin_vel_y + command[2] = ang_vel + print(f"command: {command}") + + +def handle_joystick(): + if joystick1 is None: + return + + joy_z = 0 + pygame.event.pump() + joy_y = joystick1.get_axis(1) + joy_x = joystick1.get_axis(0) + if joystick2 is not None: + joy_z = joystick2.get_axis(0) + if joy_y < 0: + lin_vel_x = (-joy_y) * COMMANDS_RANGE_X[1] + else: + lin_vel_x = -joy_y * abs(COMMANDS_RANGE_X[0]) + lin_vel_y = -joy_x * COMMANDS_RANGE_Y[1] + lin_vel_z = -joy_z * COMMANDS_RANGE_THETA[1] + + command[0] = lin_vel_x + command[1] = lin_vel_y + command[2] = lin_vel_z + print(f"command: {command}") + + +# Create a Mujoco viewer to display the model. +with mujoco.viewer.launch_passive( + model, data, show_left_ui=False, show_right_ui=False, key_callback=key_callback +) as viewer: + step = 0 + dt = model.opt.timestep + counter = 0 + new_qpos = default_qpos.copy() + while viewer.is_running(): + step_start = time.time() + handle_joystick() + counter += 1 + new_qpos[:7] = default_qpos[:7].copy() + if counter % decimation == 0: + new_qpos = default_qpos.copy() + if not all(val == 0.0 for val in command): + imitation_i = step % PRM.nb_steps_in_period + + ref_motion = PRM.get_reference_motion( + command[0], command[1], command[2], imitation_i + ) + ref_motion = np.array(ref_motion) + + if ref_motion.shape[0] == 40: + joints_pos = ref_motion[0:16] + ref_joint_pos = np.concatenate([joints_pos[:9], joints_pos[11:]]) + else: + print( + "Error: Unexpected reference motion dimension:", + ref_motion.shape, + ) + sys.exit(1) + + new_qpos = default_qpos.copy() + if new_qpos[7 : 7 + 14].shape[0] == ref_joint_pos.shape[0]: + new_qpos[7 : 7 + 14] = ref_joint_pos + else: + print( + "Error: Actuated joint dimension mismatch. Using default pose." + ) + step += 1 + else: + step = 0 + data.qpos[:] = new_qpos + + # Step the simulation to update any dependent quantities. + mujoco.mj_step(model, data) + viewer.sync() + time_until_next_step = model.opt.timestep - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(time_until_next_step) diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/runner.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/runner.py new file mode 100644 index 0000000..d73442c --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/runner.py @@ -0,0 +1,64 @@ +"""Runs training and evaluation loop for Open Duck Mini V2.""" + +import argparse + +from playground.common import randomize +from playground.common.runner import BaseRunner +from playground.open_duck_mini_v2 import joystick, standing + + +class OpenDuckMiniV2Runner(BaseRunner): + + def __init__(self, args): + super().__init__(args) + available_envs = { + "joystick": (joystick, joystick.Joystick), + "standing": (standing, standing.Standing), + } + if args.env not in available_envs: + raise ValueError(f"Unknown env {args.env}") + + self.env_file = available_envs[args.env] + + self.env_config = self.env_file[0].default_config() + self.env = self.env_file[1](task=args.task) + self.eval_env = self.env_file[1](task=args.task) + self.randomizer = randomize.domain_randomize + self.action_size = self.env.action_size + self.obs_size = int( + self.env.observation_size["state"][0] + ) # 0: state 1: privileged_state + self.restore_checkpoint_path = args.restore_checkpoint_path + print(f"Observation size: {self.obs_size}") + + +def main() -> None: + parser = argparse.ArgumentParser(description="Open Duck Mini Runner Script") + parser.add_argument( + "--output_dir", + type=str, + default="checkpoints", + help="Where to save the checkpoints", + ) + # parser.add_argument("--num_timesteps", type=int, default=300000000) + parser.add_argument("--num_timesteps", type=int, default=150000000) + parser.add_argument("--env", type=str, default="joystick", help="env") + parser.add_argument("--task", type=str, default="flat_terrain", help="Task to run") + parser.add_argument( + "--restore_checkpoint_path", + type=str, + default=None, + help="Resume training from this checkpoint", + ) + # parser.add_argument( + # "--debug", action="store_true", help="Run in debug mode with minimal parameters" + # ) + args = parser.parse_args() + + runner = OpenDuckMiniV2Runner(args) + + runner.train() + + +if __name__ == "__main__": + main() diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/standing.py b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/standing.py new file mode 100644 index 0000000..c7b1914 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/standing.py @@ -0,0 +1,661 @@ +# Copyright 2025 DeepMind Technologies Limited +# Copyright 2025 Antoine Pirrone - Steve Nguyen +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Standing task for Open Duck Mini V2. (based on Berkeley Humanoid)""" + +from typing import Any, Dict, Optional, Union +import jax +import jax.numpy as jp +from ml_collections import config_dict +from mujoco import mjx +from mujoco.mjx._src import math +import numpy as np + +from mujoco_playground._src import mjx_env +from mujoco_playground._src.collision import geoms_colliding + +from . import constants +from . import base as open_duck_mini_v2_base +from playground.common.poly_reference_motion import PolyReferenceMotion +from playground.common.rewards import ( + cost_orientation, + cost_torques, + cost_action_rate, + cost_stand_still, + reward_alive, + cost_head_pos, +) + +# if set to false, won't require the reference data to be present and won't compute the reference motions polynoms for nothing +USE_IMITATION_REWARD = False + + +def default_config() -> config_dict.ConfigDict: + return config_dict.create( + ctrl_dt=0.02, + sim_dt=0.002, + # episode_length=450, + episode_length=1000, + action_repeat=1, + action_scale=0.25, + dof_vel_scale=0.05, + history_len=0, + soft_joint_pos_limit_factor=0.95, + noise_config=config_dict.create( + level=1.0, # Set to 0.0 to disable noise. + action_min_delay=0, # env steps + action_max_delay=3, # env steps + imu_min_delay=0, # env steps + imu_max_delay=3, # env steps + scales=config_dict.create( + hip_pos=0.03, # rad, for each hip joint + knee_pos=0.05, # rad, for each knee joint + ankle_pos=0.08, # rad, for each ankle joint + joint_vel=2.5, # rad/s # Was 1.5 + gravity=0.1, + linvel=0.1, + gyro=0.05, + accelerometer=0.005, + ), + ), + reward_config=config_dict.create( + scales=config_dict.create( + # tracking_lin_vel=2.5, + # tracking_ang_vel=4.0, + orientation=-0.5, + torques=-1.0e-3, + action_rate=-0.375, # was -1.5 + stand_still=-0.3, # was -1.0 TODO try to relax this a bit ? + alive=20.0, + # imitation=1.0, + head_pos=-2.0, + ), + tracking_sigma=0.01, # was working at 0.01 + ), + push_config=config_dict.create( + enable=True, + interval_range=[5.0, 10.0], + magnitude_range=[0.1, 1.0], + ), + # lin_vel_x=[-0.1, 0.15], + # lin_vel_y=[-0.2, 0.2], + # ang_vel_yaw=[-1.0, 1.0], # [-1.0, 1.0] + neck_pitch_range=[-0.34, 1.1], + head_pitch_range=[-0.78, 0.78], + head_yaw_range=[-2.7, 2.7], + head_roll_range=[-0.5, 0.5], + head_range_factor=1.0, + ) + + +class Standing(open_duck_mini_v2_base.OpenDuckMiniV2Env): + """Standing policy""" + + def __init__( + self, + task: str = "flat_terrain", + config: config_dict.ConfigDict = default_config(), + config_overrides: Optional[Dict[str, Union[str, int, list[Any]]]] = None, + ): + super().__init__( + xml_path=constants.task_to_xml(task).as_posix(), + config=config, + config_overrides=config_overrides, + ) + self._post_init() + + def _post_init(self) -> None: + + self._init_q = jp.array(self._mj_model.keyframe("home").qpos) + self._default_actuator = self._mj_model.keyframe( + "home" + ).ctrl # ctrl of all the actual joints (no floating base and no backlash) + + if USE_IMITATION_REWARD: + self.PRM = PolyReferenceMotion( + "playground/open_duck_mini_v2/data/polynomial_coefficients.pkl" + ) + + # Note: First joint is freejoint. + # get the range of the joints + self._lowers, self._uppers = self.mj_model.jnt_range[1:].T + c = (self._lowers + self._uppers) / 2 + r = self._uppers - self._lowers + self._soft_lowers = c - 0.5 * r * self._config.soft_joint_pos_limit_factor + self._soft_uppers = c + 0.5 * r * self._config.soft_joint_pos_limit_factor + + # weights for computing the cost of each joints compared to a reference pose + self._weights = jp.array( + [ + 1.0, + 1.0, + 0.01, + 0.01, + 1.0, # left leg. + # 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, #head + 1.0, + 1.0, + 0.01, + 0.01, + 1.0, # right leg. + ] + ) + + self._njoints = self._mj_model.njnt # number of joints + self._actuators = self._mj_model.nu # number of actuators + + self._torso_body_id = self._mj_model.body(constants.ROOT_BODY).id + self._torso_mass = self._mj_model.body_subtreemass[self._torso_body_id] + self._site_id = self._mj_model.site("imu").id + + self._feet_site_id = np.array( + [self._mj_model.site(name).id for name in constants.FEET_SITES] + ) + self._floor_geom_id = self._mj_model.geom("floor").id + self._feet_geom_id = np.array( + [self._mj_model.geom(name).id for name in constants.FEET_GEOMS] + ) + + foot_linvel_sensor_adr = [] + for site in constants.FEET_SITES: + sensor_id = self._mj_model.sensor(f"{site}_global_linvel").id + sensor_adr = self._mj_model.sensor_adr[sensor_id] + sensor_dim = self._mj_model.sensor_dim[sensor_id] + foot_linvel_sensor_adr.append( + list(range(sensor_adr, sensor_adr + sensor_dim)) + ) + self._foot_linvel_sensor_adr = jp.array(foot_linvel_sensor_adr) + + # noise in the simu? + qpos_noise_scale = np.zeros(self._actuators) + + hip_ids = [ + idx for idx, j in enumerate(constants.JOINTS_ORDER_NO_HEAD) if "_hip" in j + ] + knee_ids = [ + idx for idx, j in enumerate(constants.JOINTS_ORDER_NO_HEAD) if "_knee" in j + ] + ankle_ids = [ + idx for idx, j in enumerate(constants.JOINTS_ORDER_NO_HEAD) if "_ankle" in j + ] + + qpos_noise_scale[hip_ids] = self._config.noise_config.scales.hip_pos + qpos_noise_scale[knee_ids] = self._config.noise_config.scales.knee_pos + qpos_noise_scale[ankle_ids] = self._config.noise_config.scales.ankle_pos + # qpos_noise_scale[faa_ids] = self._config.noise_config.scales.faa_pos + self._qpos_noise_scale = jp.array(qpos_noise_scale) + + def reset(self, rng: jax.Array) -> mjx_env.State: + qpos = self._init_q # the complete qpos + # print(f'DEBUG0 init qpos: {qpos}') + qvel = jp.zeros(self.mjx_model.nv) + + # init position/orientation in environment + # x=+U(-0.05, 0.05), y=+U(-0.05, 0.05), yaw=U(-3.14, 3.14). + rng, key = jax.random.split(rng) + dxy = jax.random.uniform(key, (2,), minval=-0.05, maxval=0.05) + + # floating base + base_qpos = self.get_floating_base_qpos(qpos) + base_qpos = base_qpos.at[0:2].set( + qpos[self._floating_base_qpos_addr : self._floating_base_qpos_addr + 2] + + dxy + ) # x y noise + + rng, key = jax.random.split(rng) + yaw = jax.random.uniform(key, (1,), minval=-3.14, maxval=3.14) + quat = math.axis_angle_to_quat(jp.array([0, 0, 1]), yaw) + new_quat = math.quat_mul( + qpos[self._floating_base_qpos_addr + 3 : self._floating_base_qpos_addr + 7], + quat, + ) # yaw noise + + base_qpos = base_qpos.at[3:7].set(new_quat) + + qpos = self.set_floating_base_qpos(base_qpos, qpos) + # print(f'DEBUG1 base qpos: {qpos}') + # init joint position + # qpos[7:]=*U(0.0, 0.1) + rng, key = jax.random.split(rng) + + # multiply actual joints with noise (excluding floating base and backlash) + qpos_j = self.get_actuator_joints_qpos(qpos) * jax.random.uniform( + key, (self._actuators,), minval=0.5, maxval=1.5 + ) + qpos = self.set_actuator_joints_qpos(qpos_j, qpos) + # print(f'DEBUG2 joint qpos: {qpos}') + # init joint vel + # d(xyzrpy)=U(-0.05, 0.05) + rng, key = jax.random.split(rng) + # qvel = qvel.at[self._floating_base_qvel_addr : self._floating_base_qvel_addr + 6].set( + # jax.random.uniform(key, (6,), minval=-0.5, maxval=0.5) + # ) + + qvel = self.set_floating_base_qvel( + jax.random.uniform(key, (6,), minval=-0.5, maxval=0.5), qvel + ) + # print(f'DEBUG3 base qvel: {qvel}') + ctrl = self.get_actuator_joints_qpos(qpos) + # print(f'DEBUG4 ctrl: {ctrl}') + data = mjx_env.init(self.mjx_model, qpos=qpos, qvel=qvel, ctrl=ctrl) + rng, cmd_rng = jax.random.split(rng) + cmd = self.sample_command(cmd_rng) + + # Sample push interval. + rng, push_rng = jax.random.split(rng) + push_interval = jax.random.uniform( + push_rng, + minval=self._config.push_config.interval_range[0], + maxval=self._config.push_config.interval_range[1], + ) + push_interval_steps = jp.round(push_interval / self.dt).astype(jp.int32) + + if USE_IMITATION_REWARD: + current_reference_motion = self.PRM.get_reference_motion( + cmd[0], cmd[1], cmd[2], 0 + ) + else: + current_reference_motion = jp.zeros(0) + + info = { + "rng": rng, + "step": 0, + "command": cmd, + "last_act": jp.zeros(self.mjx_model.nu), + "last_last_act": jp.zeros(self.mjx_model.nu), + "last_last_last_act": jp.zeros(self.mjx_model.nu), + "motor_targets": jp.zeros(self.mjx_model.nu), + "feet_air_time": jp.zeros(2), + "last_contact": jp.zeros(2, dtype=bool), + "swing_peak": jp.zeros(2), + # Push related. + "push": jp.array([0.0, 0.0]), + "push_step": 0, + "push_interval_steps": push_interval_steps, + # History related. + "action_history": jp.zeros( + self._config.noise_config.action_max_delay * self._actuators + ), + "imu_history": jp.zeros(self._config.noise_config.imu_max_delay * 3), + # imitation related + "imitation_i": 0, + "current_reference_motion": current_reference_motion, + } + + metrics = {} + for k, v in self._config.reward_config.scales.items(): + if v != 0: + if v > 0: + metrics[f"reward/{k}"] = jp.zeros(()) + else: + metrics[f"cost/{k}"] = jp.zeros(()) + metrics["swing_peak"] = jp.zeros(()) + + contact = jp.array( + [ + geoms_colliding(data, geom_id, self._floor_geom_id) + for geom_id in self._feet_geom_id + ] + ) + obs = self._get_obs(data, info, contact) + reward, done = jp.zeros(2) + return mjx_env.State(data, obs, reward, done, metrics, info) + + def step(self, state: mjx_env.State, action: jax.Array) -> mjx_env.State: + + if USE_IMITATION_REWARD: + state.info["imitation_i"] += 1 + state.info["imitation_i"] = ( + state.info["imitation_i"] % self.PRM.nb_steps_in_period + ) # not critical, is already moduloed in get_reference_motion + else: + state.info["imitation_i"] = 0 + + if USE_IMITATION_REWARD: + state.info["current_reference_motion"] = self.PRM.get_reference_motion( + state.info["command"][0], + state.info["command"][1], + state.info["command"][2], + state.info["imitation_i"], + ) + else: + state.info["current_reference_motion"] = jp.zeros(0) + + state.info["rng"], push1_rng, push2_rng, action_delay_rng = jax.random.split( + state.info["rng"], 4 + ) + + # Handle action delay + action_history = ( + jp.roll(state.info["action_history"], self._actuators) + .at[: self._actuators] + .set(action) + ) + state.info["action_history"] = action_history + action_idx = jax.random.randint( + action_delay_rng, + (1,), + minval=self._config.noise_config.action_min_delay, + maxval=self._config.noise_config.action_max_delay, + ) + action_w_delay = action_history.reshape((-1, self._actuators))[ + action_idx[0] + ] # action with delay + + push_theta = jax.random.uniform(push1_rng, maxval=2 * jp.pi) + push_magnitude = jax.random.uniform( + push2_rng, + minval=self._config.push_config.magnitude_range[0], + maxval=self._config.push_config.magnitude_range[1], + ) + push = jp.array([jp.cos(push_theta), jp.sin(push_theta)]) + push *= ( + jp.mod(state.info["push_step"] + 1, state.info["push_interval_steps"]) == 0 + ) + push *= self._config.push_config.enable + qvel = state.data.qvel + qvel = qvel.at[ + self._floating_base_qvel_addr : self._floating_base_qvel_addr + 2 + ].set( + push * push_magnitude + + qvel[self._floating_base_qvel_addr : self._floating_base_qvel_addr + 2] + ) # floating base x,y + data = state.data.replace(qvel=qvel) + state = state.replace(data=data) + + motor_targets = ( + self._default_actuator + action_w_delay * self._config.action_scale + ) + data = mjx_env.step(self.mjx_model, state.data, motor_targets, self.n_substeps) + state.info["motor_targets"] = motor_targets + + contact = jp.array( + [ + geoms_colliding(data, geom_id, self._floor_geom_id) + for geom_id in self._feet_geom_id + ] + ) + contact_filt = contact | state.info["last_contact"] + first_contact = (state.info["feet_air_time"] > 0.0) * contact_filt + state.info["feet_air_time"] += self.dt + p_f = data.site_xpos[self._feet_site_id] + p_fz = p_f[..., -1] + state.info["swing_peak"] = jp.maximum(state.info["swing_peak"], p_fz) + + obs = self._get_obs(data, state.info, contact) + done = self._get_termination(data) + + rewards = self._get_reward( + data, action, state.info, state.metrics, done, first_contact, contact + ) + # FIXME + rewards = { + k: v * self._config.reward_config.scales[k] for k, v in rewards.items() + } + reward = jp.clip(sum(rewards.values()) * self.dt, 0.0, 10000.0) + # jax.debug.print('STEP REWARD: {}',reward) + state.info["push"] = push + state.info["step"] += 1 + state.info["push_step"] += 1 + state.info["last_last_last_act"] = state.info["last_last_act"] + state.info["last_last_act"] = state.info["last_act"] + state.info["last_act"] = action + state.info["rng"], cmd_rng = jax.random.split(state.info["rng"]) + state.info["command"] = jp.where( + state.info["step"] > 500, + self.sample_command(cmd_rng), + state.info["command"], + ) + state.info["step"] = jp.where( + done | (state.info["step"] > 500), + 0, + state.info["step"], + ) + state.info["feet_air_time"] *= ~contact + state.info["last_contact"] = contact + state.info["swing_peak"] *= ~contact + for k, v in rewards.items(): + rew_scale = self._config.reward_config.scales[k] + if rew_scale != 0: + if rew_scale > 0: + state.metrics[f"reward/{k}"] = v + else: + state.metrics[f"cost/{k}"] = -v + state.metrics["swing_peak"] = jp.mean(state.info["swing_peak"]) + + done = done.astype(reward.dtype) + state = state.replace(data=data, obs=obs, reward=reward, done=done) + return state + + def _get_termination(self, data: mjx.Data) -> jax.Array: + fall_termination = self.get_gravity(data)[-1] < 0.0 + return fall_termination | jp.isnan(data.qpos).any() | jp.isnan(data.qvel).any() + + def _get_obs( + self, data: mjx.Data, info: dict[str, Any], contact: jax.Array + ) -> mjx_env.Observation: + + gyro = self.get_gyro(data) + info["rng"], noise_rng = jax.random.split(info["rng"]) + noisy_gyro = ( + gyro + + (2 * jax.random.uniform(noise_rng, shape=gyro.shape) - 1) + * self._config.noise_config.level + * self._config.noise_config.scales.gyro + ) + + accelerometer = self.get_accelerometer(data) + info["rng"], noise_rng = jax.random.split(info["rng"]) + noisy_accelerometer = ( + accelerometer + + (2 * jax.random.uniform(noise_rng, shape=accelerometer.shape) - 1) + * self._config.noise_config.level + * self._config.noise_config.scales.accelerometer + ) + + gravity = data.site_xmat[self._site_id].T @ jp.array([0, 0, -1]) + info["rng"], noise_rng = jax.random.split(info["rng"]) + noisy_gravity = ( + gravity + + (2 * jax.random.uniform(noise_rng, shape=gravity.shape) - 1) + * self._config.noise_config.level + * self._config.noise_config.scales.gravity + ) + + # Handle IMU delay + imu_history = jp.roll(info["imu_history"], 3).at[:3].set(noisy_gravity) + info["imu_history"] = imu_history + imu_idx = jax.random.randint( + noise_rng, + (1,), + minval=self._config.noise_config.imu_min_delay, + maxval=self._config.noise_config.imu_max_delay, + ) + noisy_gravity = imu_history.reshape((-1, 3))[imu_idx[0]] + + # joint_angles = data.qpos[7:] + + # Handling backlash + joint_angles = self.get_actuator_joints_qpos(data.qpos) + joint_backlash = self.get_actuator_backlash_qpos(data.qpos) + + for i in self.backlash_idx_to_add: + joint_backlash = jp.insert(joint_backlash, i, 0) + + joint_angles = joint_angles + joint_backlash + + info["rng"], noise_rng = jax.random.split(info["rng"]) + noisy_joint_angles = ( + joint_angles + + (2.0 * jax.random.uniform(noise_rng, shape=joint_angles.shape) - 1.0) + * self._config.noise_config.level + * self._qpos_noise_scale + ) + + # joint_vel = data.qvel[6:] + joint_vel = self.get_actuator_joints_qvel(data.qvel) + info["rng"], noise_rng = jax.random.split(info["rng"]) + noisy_joint_vel = ( + joint_vel + + (2.0 * jax.random.uniform(noise_rng, shape=joint_vel.shape) - 1.0) + * self._config.noise_config.level + * self._config.noise_config.scales.joint_vel + ) + + linvel = self.get_local_linvel(data) + # info["rng"], noise_rng = jax.random.split(info["rng"]) + # noisy_linvel = ( + # linvel + # + (2 * jax.random.uniform(noise_rng, shape=linvel.shape) - 1) + # * self._config.noise_config.level + # * self._config.noise_config.scales.linvel + # ) + + state = jp.hstack( + [ + # noisy_linvel, # 3 + # noisy_gyro, # 3 + # noisy_gravity, # 3 + noisy_gyro, # 3 + noisy_accelerometer, # 3 + info["command"], # 3 + noisy_joint_angles - self._default_actuator, # 10 + noisy_joint_vel * self._config.dof_vel_scale, # 10 + info["last_act"], # 10 + info["last_last_act"], # 10 + info["last_last_last_act"], # 10 + contact, # 2 + info["current_reference_motion"], + ] + ) + + accelerometer = self.get_accelerometer(data) + global_angvel = self.get_global_angvel(data) + feet_vel = data.sensordata[self._foot_linvel_sensor_adr].ravel() + root_height = data.qpos[self._floating_base_qpos_addr + 2] + + privileged_state = jp.hstack( + [ + state, + gyro, # 3 + accelerometer, # 3 + gravity, # 3 + linvel, # 3 + global_angvel, # 3 + joint_angles - self._default_actuator, + joint_vel, + root_height, # 1 + data.actuator_force, # 10 + contact, # 2 + feet_vel, # 4*3 + info["feet_air_time"], # 2 + info["current_reference_motion"], + ] + ) + + return { + "state": state, + "privileged_state": privileged_state, + } + + def _get_reward( + self, + data: mjx.Data, + action: jax.Array, + info: dict[str, Any], + metrics: dict[str, Any], + done: jax.Array, + first_contact: jax.Array, + contact: jax.Array, + ) -> dict[str, jax.Array]: + del metrics # Unused. + + ret = { + "orientation": cost_orientation(self.get_gravity(data)), + "torques": cost_torques(data.actuator_force), + "action_rate": cost_action_rate(action, info["last_act"]), + "alive": reward_alive(), + "stand_still": cost_stand_still( + # info["command"], data.qpos[7:], data.qvel[6:], self._default_pose + info["command"], + self.get_actuator_joints_qpos(data.qpos), + self.get_actuator_joints_qvel(data.qvel), + self._default_actuator, + True + ), + "head_pos": cost_head_pos( + self.get_actuator_joints_qpos(data.qpos), + self.get_actuator_joints_qvel(data.qvel), + info["command"], + ), + } + + return ret + + def sample_command(self, rng: jax.Array) -> jax.Array: + rng1, rng2, rng3, rng4, rng5, rng6, rng7, rng8 = jax.random.split(rng, 8) + + # lin_vel_x = jax.random.uniform( + # rng1, minval=self._config.lin_vel_x[0], maxval=self._config.lin_vel_x[1] + # ) + # lin_vel_y = jax.random.uniform( + # rng2, minval=self._config.lin_vel_y[0], maxval=self._config.lin_vel_y[1] + # ) + # ang_vel_yaw = jax.random.uniform( + # rng3, + # minval=self._config.ang_vel_yaw[0], + # maxval=self._config.ang_vel_yaw[1], + # ) + + neck_pitch = jax.random.uniform( + rng5, + minval=self._config.neck_pitch_range[0] * self._config.head_range_factor, + maxval=self._config.neck_pitch_range[1] * self._config.head_range_factor, + ) + + head_pitch = jax.random.uniform( + rng6, + minval=self._config.head_pitch_range[0] * self._config.head_range_factor, + maxval=self._config.head_pitch_range[1] * self._config.head_range_factor, + ) + + head_yaw = jax.random.uniform( + rng7, + minval=self._config.head_yaw_range[0] * self._config.head_range_factor, + maxval=self._config.head_yaw_range[1] * self._config.head_range_factor, + ) + + head_roll = jax.random.uniform( + rng8, + minval=self._config.head_roll_range[0] * self._config.head_range_factor, + maxval=self._config.head_roll_range[1] * self._config.head_range_factor, + ) + + # With 10% chance, set everything to zero. + return jp.where( + jax.random.bernoulli(rng4, p=0.1), + jp.zeros(7), + jp.hstack( + [ + 0.0, + 0.0, + 0.0, + neck_pitch, + head_pitch, + head_yaw, + head_roll, + ] + ), + ) diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/antenna.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/antenna.part new file mode 100644 index 0000000..d004998 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/antenna.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MmXLbiyIJZ1T9tiX3", + "isStandardContent": false, + "name": "antenna <1>", + "partId": "RGPD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/antenna.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/antenna.stl new file mode 100644 index 0000000..091f332 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/antenna.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_back.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_back.part new file mode 100644 index 0000000..0c0e72c --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_back.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MfCA08JspCUYCoOzy", + "isStandardContent": false, + "name": "body_back <1>", + "partId": "RqKD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_back.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_back.stl new file mode 100644 index 0000000..d29e185 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_back.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_front.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_front.part new file mode 100644 index 0000000..a61b01f --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_front.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MAILAHna+4EMDjwEB", + "isStandardContent": false, + "name": "body_front <1>", + "partId": "RbKD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_front.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_front.stl new file mode 100644 index 0000000..c277c49 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_front.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_middle_bottom.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_middle_bottom.part new file mode 100644 index 0000000..a8db44d --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_middle_bottom.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Mi7/ZBgz7j8FhNN8n", + "isStandardContent": false, + "name": "body_middle_bottom <1>", + "partId": "R/KH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_middle_bottom.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_middle_bottom.stl new file mode 100644 index 0000000..70dfe23 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_middle_bottom.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_middle_top.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_middle_top.part new file mode 100644 index 0000000..402df2b --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_middle_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MwLs2m7WqJdIp5rCM", + "isStandardContent": false, + "name": "body_middle_top <1>", + "partId": "R/KD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_middle_top.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_middle_top.stl new file mode 100644 index 0000000..9cae6ca Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/body_middle_top.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_bottom_pla.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_bottom_pla.part new file mode 100644 index 0000000..0926dae --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_bottom_pla.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MbWaXbNFhwXrvfPr1", + "isStandardContent": false, + "name": "foot_bottom_pla <1>", + "partId": "R7GH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_bottom_pla.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_bottom_pla.stl new file mode 100644 index 0000000..9c45676 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_bottom_pla.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_bottom_tpu.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_bottom_tpu.part new file mode 100644 index 0000000..88c9e3c --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_bottom_tpu.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MSRJjnMROc2yqGlBV", + "isStandardContent": false, + "name": "foot_bottom_tpu <1>", + "partId": "R7GD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_bottom_tpu.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_bottom_tpu.stl new file mode 100644 index 0000000..f9413d8 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_bottom_tpu.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_side.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_side.part new file mode 100644 index 0000000..fb58fd5 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_side.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M9QvdrmpVd96BQQ3J", + "isStandardContent": false, + "name": "foot_side <1>", + "partId": "RsGD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_side.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_side.stl new file mode 100644 index 0000000..47d22d5 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_side.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_top.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_top.part new file mode 100644 index 0000000..ca57ade --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MryNg763VjsN4xBWU", + "isStandardContent": false, + "name": "foot_top <1>", + "partId": "RsGH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_top.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_top.stl new file mode 100644 index 0000000..7130ec9 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/foot_top.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head.part new file mode 100644 index 0000000..35281cf --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MGPnRRHSCLRuHbf9s", + "isStandardContent": false, + "name": "head <1>", + "partId": "RXMD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head.stl new file mode 100644 index 0000000..5a9eafb Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_bot_sheet.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_bot_sheet.part new file mode 100644 index 0000000..31f420d --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_bot_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MB88Zh5nm8uQRQJB5", + "isStandardContent": false, + "name": "head_bot_sheet <1>", + "partId": "RwMD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_bot_sheet.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_bot_sheet.stl new file mode 100644 index 0000000..1ee7c77 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_bot_sheet.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_pitch_to_yaw.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_pitch_to_yaw.part new file mode 100644 index 0000000..c225e32 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_pitch_to_yaw.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MMYrJ8GU6Tulh9Ezl", + "isStandardContent": false, + "name": "head_pitch_to_yaw <1>", + "partId": "RGCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_pitch_to_yaw.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_pitch_to_yaw.stl new file mode 100644 index 0000000..ca21bb2 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_pitch_to_yaw.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_yaw_to_roll.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_yaw_to_roll.part new file mode 100644 index 0000000..54ccbca --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_yaw_to_roll.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MSSQrz89LHCpx23/c", + "isStandardContent": false, + "name": "head_yaw_to_roll <1>", + "partId": "RyCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_yaw_to_roll.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_yaw_to_roll.stl new file mode 100644 index 0000000..3ae2a1b Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/head_yaw_to_roll.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/hfield.png b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/hfield.png new file mode 100644 index 0000000..62af27a Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/hfield.png differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_antenna_holder.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_antenna_holder.part new file mode 100644 index 0000000..b5a4507 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_antenna_holder.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MqpShx/iKn+WMc6nz", + "isStandardContent": false, + "name": "left_antenna_holder <1>", + "partId": "R3PL", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_antenna_holder.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_antenna_holder.stl new file mode 100644 index 0000000..cb24ed2 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_antenna_holder.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_cache.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_cache.part new file mode 100644 index 0000000..bf8eda8 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_cache.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Mh206kD+LDwOu9+3I", + "isStandardContent": false, + "name": "left_cache <1>", + "partId": "RELD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_cache.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_cache.stl new file mode 100644 index 0000000..c31866f Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_cache.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_knee_to_ankle_left_sheet.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_knee_to_ankle_left_sheet.part new file mode 100644 index 0000000..1acf6ef --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_knee_to_ankle_left_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MkexR4anxdIDvoNnP", + "isStandardContent": false, + "name": "left_knee_to_ankle_left_sheet <1>", + "partId": "RyDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_knee_to_ankle_left_sheet.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_knee_to_ankle_left_sheet.stl new file mode 100644 index 0000000..55bdd14 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_knee_to_ankle_left_sheet.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_knee_to_ankle_right_sheet.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_knee_to_ankle_right_sheet.part new file mode 100644 index 0000000..12e2878 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_knee_to_ankle_right_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MFSYykuSOyFPl8MEE", + "isStandardContent": false, + "name": "left_knee_to_ankle_right_sheet <1>", + "partId": "RyDH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_knee_to_ankle_right_sheet.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_knee_to_ankle_right_sheet.stl new file mode 100644 index 0000000..69a1642 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_knee_to_ankle_right_sheet.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_roll_to_pitch.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_roll_to_pitch.part new file mode 100644 index 0000000..a6e7038 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_roll_to_pitch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Mg7gXWiWLFdTkX/WZ", + "isStandardContent": false, + "name": "left_roll_to_pitch <1>", + "partId": "RRFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_roll_to_pitch.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_roll_to_pitch.stl new file mode 100644 index 0000000..de35dd2 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/left_roll_to_pitch.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/leg_spacer.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/leg_spacer.part new file mode 100644 index 0000000..73e482a --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/leg_spacer.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M1AJs3nTP2bD6DgAF", + "isStandardContent": false, + "name": "leg_spacer <1>", + "partId": "RfED", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/leg_spacer.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/leg_spacer.stl new file mode 100644 index 0000000..c4fbfae Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/leg_spacer.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/neck_left_sheet.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/neck_left_sheet.part new file mode 100644 index 0000000..83ea9f5 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/neck_left_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M8Je0JZqWOimCS7cY", + "isStandardContent": false, + "name": "neck_left_sheet <1>", + "partId": "RgLD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/neck_left_sheet.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/neck_left_sheet.stl new file mode 100644 index 0000000..4b437db Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/neck_left_sheet.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/neck_right_sheet.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/neck_right_sheet.part new file mode 100644 index 0000000..1c466c1 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/neck_right_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M8fJckSPn4u+ct8pO", + "isStandardContent": false, + "name": "neck_right_sheet <1>", + "partId": "RgLH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/neck_right_sheet.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/neck_right_sheet.stl new file mode 100644 index 0000000..85724a5 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/neck_right_sheet.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_antenna_holder.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_antenna_holder.part new file mode 100644 index 0000000..53536db --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_antenna_holder.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M/p8PGArLA1nrXwe8", + "isStandardContent": false, + "name": "right_antenna_holder <1>", + "partId": "R0OD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_antenna_holder.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_antenna_holder.stl new file mode 100644 index 0000000..02ed801 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_antenna_holder.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_cache.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_cache.part new file mode 100644 index 0000000..7edb89a --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_cache.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MBtFsLan/52XkruX7", + "isStandardContent": false, + "name": "right_cache <1>", + "partId": "R3PH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_cache.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_cache.stl new file mode 100644 index 0000000..b09c3f2 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_cache.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_roll_to_pitch.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_roll_to_pitch.part new file mode 100644 index 0000000..eb8ab4e --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_roll_to_pitch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Mvz4SlAytL1YUO5L4", + "isStandardContent": false, + "name": "right_roll_to_pitch <1>", + "partId": "R3PD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_roll_to_pitch.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_roll_to_pitch.stl new file mode 100644 index 0000000..5c6e7c8 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/right_roll_to_pitch.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/roll_motor_bottom.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/roll_motor_bottom.part new file mode 100644 index 0000000..1db25e3 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/roll_motor_bottom.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MjsDdlagt/LopvMjR", + "isStandardContent": false, + "name": "roll_motor_bottom <1>", + "partId": "RGHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/roll_motor_bottom.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/roll_motor_bottom.stl new file mode 100644 index 0000000..fefef26 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/roll_motor_bottom.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/roll_motor_top.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/roll_motor_top.part new file mode 100644 index 0000000..8eefe2c --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/roll_motor_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MoZYOvTkbbGLGW0df", + "isStandardContent": false, + "name": "roll_motor_top <1>", + "partId": "RyHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/roll_motor_top.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/roll_motor_top.stl new file mode 100644 index 0000000..31415de Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/roll_motor_top.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/trunk_bottom.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/trunk_bottom.part new file mode 100644 index 0000000..e471cbf --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/trunk_bottom.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MMYHrJ4hqH24cpa8I", + "isStandardContent": false, + "name": "trunk_bottom <1>", + "partId": "RvJH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/trunk_bottom.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/trunk_bottom.stl new file mode 100644 index 0000000..02dc068 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/trunk_bottom.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/trunk_top.part b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/trunk_top.part new file mode 100644 index 0000000..97a8730 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/trunk_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "78f84c802a6a701851609660", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MNg8avQSkDpa0UkRX", + "isStandardContent": false, + "name": "trunk_top <1>", + "partId": "RvJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/trunk_top.stl b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/trunk_top.stl new file mode 100644 index 0000000..99e98b8 Binary files /dev/null and b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/assets/trunk_top.stl differ diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/config.json b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/config.json new file mode 100644 index 0000000..a26fffd --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/config.json @@ -0,0 +1,51 @@ +{ + // NEED TO WRAP all in a body named "base" and add site + // + // and comment the default kp frictionloss etc + "documentId": "64074dfcfa379b37d8a47762", + "outputFormat": "mujoco", + "assemblyName": "Open_Duck_Mini_v2", + "robot_name": "open_duck_mini_v2", + "output_filename": "open_duck_mini_v2", + // Simplify STL meshes (default: false) + "simplify_stls": true, + // Maximum size of the STL files in MB (default: 3) + "max_stl_size": 0.005, + // "collisions_as_visual": true, + "ignore": { + "wj*": "visual", + "drive_palonier": "visual", + "passive_palonier": "visual", + "bulb": "visual", + "cam": "visual", + "sg90": "visual", + "head_roll_mount": "visual", + "raspberrypizerow": "visual", + "speaker_stand": "visual", + "speaker_interface": "visual", + "holder": "visual", + "cell": "visual", + "bms": "visual", + "usb_c_charger": "visual", + "battery_pack_lid": "visual", + "right_eye": "visual", + "left_eye": "visual", + "glass": "visual", + "full_speaker": "visual", + "roll_bearing": "visual", + "power_switch": "visual", + "board": "visual", + "bno055": "visual", + "*": "collision", + "!foot_bottom_tpu": "collision" + }, + "joint_properties": { + "default": { + "class": "sts3215" + } + }, + "additional_xml": [ + "sensors.xml", + "joints_properties.xml" + ] +} \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/joints_properties.xml b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/joints_properties.xml new file mode 100644 index 0000000..0e2b2a5 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/joints_properties.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/open_duck_mini_v2.xml b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/open_duck_mini_v2.xml new file mode 100644 index 0000000..86204bf --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/open_duck_mini_v2.xml @@ -0,0 +1,503 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/open_duck_mini_v2_backlash.xml b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/open_duck_mini_v2_backlash.xml new file mode 100644 index 0000000..94fbc64 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/open_duck_mini_v2_backlash.xml @@ -0,0 +1,514 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/scene_flat_terrain.xml b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/scene_flat_terrain.xml new file mode 100644 index 0000000..e150016 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/scene_flat_terrain.xml @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/scene_flat_terrain_backlash.xml b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/scene_flat_terrain_backlash.xml new file mode 100644 index 0000000..7dfa5bb --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/scene_flat_terrain_backlash.xml @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/scene_rough_terrain_backlash.xml b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/scene_rough_terrain_backlash.xml new file mode 100644 index 0000000..941db19 --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/scene_rough_terrain_backlash.xml @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/sensors.xml b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/sensors.xml new file mode 100644 index 0000000..8652d1a --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/playground/open_duck_mini_v2/xmls/sensors.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_Playground-main/Open_Duck_Playground-main/pyproject.toml b/Open_Duck_Playground-main/Open_Duck_Playground-main/pyproject.toml new file mode 100644 index 0000000..67b2dea --- /dev/null +++ b/Open_Duck_Playground-main/Open_Duck_Playground-main/pyproject.toml @@ -0,0 +1,23 @@ +[project] +name = "Open_Duck_Playground" +version = "0.1.0" +description = "Add your description here" +readme = "README.md" +requires-python = ">=3.11" +dependencies = [ + "framesviewer>=1.0.2", + "jax[cuda12]>=0.5.0", + "jaxlib>=0.5.0", + "jaxtyping>=0.2.38", + "matplotlib>=3.10.0", + "mediapy>=1.2.2", + "onnxruntime>=1.20.1", + "playground>=0.0.3", + "pygame>=2.6.1", + "tensorflow>=2.18.0", + "tf2onnx>=1.16.1", +] +[build-system] +requires = ["setuptools"] +[tool.setuptools] +packages = ["playground"] \ No newline at end of file diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/.gitattributes b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/.gitattributes new file mode 100644 index 0000000..4b3697f --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/.gitattributes @@ -0,0 +1,2 @@ +*.part filter=lfs diff=lfs merge=lfs -text +*.stl filter=lfs diff=lfs merge=lfs -text diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/.gitignore b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/.gitignore new file mode 100644 index 0000000..1e10f07 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/.gitignore @@ -0,0 +1,2 @@ +uv.lock +open_duck_reference_motion_generator/__pycache__/ \ No newline at end of file diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/README.md b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/README.md new file mode 100644 index 0000000..347a1df --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/README.md @@ -0,0 +1,78 @@ +![banner](https://github.com/user-attachments/assets/f445e373-74fc-413b-aa73-0f17c76b1171) + + +# Open Duck Reference Motion Generator + +Open Duck project's reference motion generator for imitation learning, based on [Placo](https://github.com/Rhoban/placo). + +The reference motions are used in two RL works, one using mujoco playground [here](https://github.com/SteveNguyen/openduckminiv2_playground) and another using Isaac Gym [here](https://github.com/rimim/AWD) + +> This repo uses `git-lfs`. Install it with `sudo apt install git-lfs` before cloning the repo + +## Installation + +Install uv + +```bash +curl -LsSf https://astral.sh/uv/install.sh | sh +``` + +## Usage + +### Generate motions + +```bash +uv run scripts/auto_waddle.py (-j?) --duck ["go_bdx", "open_duck_mini", "open_duck_mini_v2"] (--num <> / --sweep) --output_dir <> +``` + +> If you have an error like this : `Failed to determine STL storage representation for <...> Hint: the mesh directory may be wrong`, just run `git lfs pull` + +Args : +- `-j?` number of jobs. If `j` is not specified, will run sequentially. If `j` is specified without a number, your computer will crash :) (runs with all available cores). Use `j4` for example +- `--duck` selects the duck type +- `--sweep` generates all combinations of motion within the ranges specified in `Open_Duck_reference_motion_generator/open_duck_reference_motion_generator/robots//auto_gait.json` +- `--num` generates random motions +- `--output_dir` self explanatory + +For example, to generate all the reference motion for Open Duck Mini, run : + +```bash +uv run scripts/auto_waddle.py -j8 --duck open_duck_mini_v2 --sweep +``` + +This will write in a directory called `recordings/` + +### Fit polynomials + +This will generate `polynomial_coefficients.pkl` +```bash +uv run scripts/fit_poly.py --ref_motion recordings/ +``` + +To plot : + +```bash +uv run scripts/plot_poly_fit.py --coefficients polynomial_coefficients.pkl +``` + +### Replay + +```bash +uv run scripts/replay_motion.py -f recordings/.json +``` + +### Playground + +```bash +uv run open_duck_reference_motion_generator/gait_playground.py --duck ["go_bdx", "open_duck_mini", "open_duck_mini_v2"] +``` + +## TODO + +- The robots descriptions should be in a separate repository and imported as a submodule. This would help having duplicates of the same files, or with different versions etc. +- Validate that we can train policies with these motions (might have broken something during the port...) +- Fix small bugs in gait_playground (refreshing, changing robots ...) +- Nicer visualization ? Directly visualize using meshcat maybe. +- A document to specify the reference motion format, if someone wants to convert some mocap data to use for the imitation reward ? +- The repo duck themed, but it could be a generic motion generator based on placo for biped robots (will add sigmaban at some point) + - Sub TODO : explain how to add a new robot diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/gait_generator.py b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/gait_generator.py new file mode 100644 index 0000000..a19e519 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/gait_generator.py @@ -0,0 +1,465 @@ +import argparse +import json +import os +import time +import webbrowser +import threading +import numpy as np +from placo_utils.visualization import footsteps_viz, robot_frame_viz, robot_viz +from scipy.spatial.transform import Rotation as R + +from placo_walk_engine import PlacoWalkEngine + + +def open_browser(): + try: + webbrowser.open_new("http://127.0.0.1:7000/static/") + except: + print("Failed to open the default browser. Trying Google Chrome.") + try: + webbrowser.get("google-chrome").open_new("http://127.0.0.1:7000/static/") + except: + print( + "Failed to open Google Chrome. Make sure it's installed and accessible." + ) + + +class RoundingFloat(float): + __repr__ = staticmethod(lambda x: format(x, ".5f")) + + +parser = argparse.ArgumentParser() +parser.add_argument("-n", "--name", type=str, required=True) +script_path = os.path.dirname(os.path.abspath(__file__)) +parser.add_argument( + "-o", "--output_dir", type=str, default=os.path.join(script_path, "recordings") +) +parser.add_argument("--dx", type=float, default=0) +parser.add_argument("--dy", type=float, default=0) +parser.add_argument("--dtheta", type=float, default=0) +parser.add_argument("--double_support_ratio", type=float, default=None) +parser.add_argument("--startend_double_support_ratio", type=float, default=None) +parser.add_argument("--planned_timesteps", type=float, default=None) +parser.add_argument("--replan_timesteps", type=float, default=None) +parser.add_argument("--walk_com_height", type=float, default=None) +parser.add_argument("--walk_foot_height", type=float, default=None) +parser.add_argument("--walk_trunk_pitch", type=float, default=None) +parser.add_argument("--walk_foot_rise_ratio", type=float, default=None) +parser.add_argument("--single_support_duration", type=float, default=None) +parser.add_argument("--single_support_timesteps", type=float, default=None) +parser.add_argument("--foot_length", type=float, default=None) +parser.add_argument("--feet_spacing", type=float, default=None) +parser.add_argument("--zmp_margin", type=float, default=None) +parser.add_argument("--foot_zmp_target_x", type=float, default=None) +parser.add_argument("--foot_zmp_target_y", type=float, default=None) +parser.add_argument("--walk_max_dtheta", type=float, default=None) +parser.add_argument("--walk_max_dy", type=float, default=None) +parser.add_argument("--walk_max_dx_forward", type=float, default=None) +parser.add_argument("--walk_max_dx_backward", type=float, default=None) +parser.add_argument("-l", "--length", type=int, default=10) +parser.add_argument("-m", "--meshcat_viz", action="store_true", default=False) +parser.add_argument( + "--duck", + choices=["go_bdx", "open_duck_mini", "open_duck_mini_v2"], + help="Duck type", + required=True, +) +parser.add_argument("--debug", action="store_true", default=False) +parser.add_argument("--preset", type=str, default="") +parser.add_argument( + "-s", + "--skip_warmup", + action="store_true", + default=False, + help="don't record warmup motion", +) +parser.add_argument( + "--stand", + action="store_true", + default=False, + help="hack to record a standing pose", +) +args = parser.parse_args() +args.hardware = True + +FPS = 50 # 50 for mujoco playground, 30 for AWD +MESHCAT_FPS = 20 +DISPLAY_MESHCAT = args.meshcat_viz + +# For IsaacGymEnvs (OUTDATED) +# [root position, root orientation, joint poses (e.g. rotations)] +# [x, y, z, qw, qx, qy, qz, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14] + +# For AWD and amp for hardware +# [root position, root orientation, joint poses (e.g. rotations), target toe positions, linear velocity, angular velocity, joint velocities, target toe velocities] +# [x, y, z, qw, qx, qy, qz, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14, l_toe_x, l_toe_y, l_toe_z, r_toe_x, r_toe_y, r_toe_z, lin_vel_x, lin_vel_y, lin_vel_z, ang_vel_x, ang_vel_y, ang_vel_z, j1_vel, j2_vel, j3_vel, j4_vel, j5_vel, j6_vel, j7_vel, j8_vel, j9_vel, j10_vel, j11_vel, j12_vel, j13_vel, j14_vel, l_toe_vel_x, l_toe_vel_y, l_toe_vel_z, r_toe_vel_x, r_toe_vel_y, r_toe_vel_z] + +episode = { + "LoopMode": "Wrap", + "FPS": FPS, + "FrameDuration": np.around(1 / FPS, 4), + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Joints": [], + "Vel_x": [], + "Vel_y": [], + "Yaw": [], + "Placo": [], + "Frame_offset": [], + "Frame_size": [], + "Frames": [], + "MotionWeight": 1, +} +if args.debug: + episode["Debug_info"] = [] + +script_path = os.path.dirname(os.path.abspath(__file__)) +robot = args.duck +robot_urdf = f"{robot}.urdf" +asset_path = os.path.join(script_path, f"robots/{robot}") + +preset_filename = args.preset +filename = os.path.join(asset_path, "placo_defaults.json") +if preset_filename: + if os.path.exists(preset_filename): + filename = preset_filename + else: + print(f"No such file: {preset_filename}") +with open(filename, "r") as f: + gait_parameters = json.load(f) + print(f"gait_parameters {gait_parameters}") + +args.dx = gait_parameters["dx"] +args.dy = gait_parameters["dy"] +args.dtheta = gait_parameters["dtheta"] + +pwe = PlacoWalkEngine(asset_path, robot_urdf, gait_parameters) + +first_joints_positions = list(pwe.get_angles().values()) +first_T_world_fbase = pwe.robot.get_T_world_fbase() +first_T_world_leftFoot = pwe.robot.get_T_world_left() +first_T_world_rightFoot = pwe.robot.get_T_world_right() + +pwe.set_traj(args.dx, args.dy, args.dtheta + 0.00955) +if DISPLAY_MESHCAT: + viz = robot_viz(pwe.robot) + threading.Timer(1, open_browser).start() +DT = 0.001 +start = time.time() + +last_record = 0 +last_meshcat_display = 0 +prev_root_position = [0, 0, 0] +prev_root_orientation_quat = None +prev_root_orientation_euler = [0, 0, 0] +prev_left_toe_pos = [0, 0, 0] +prev_right_toe_pos = [0, 0, 0] +prev_joints_positions = None +i = 0 +prev_initialized = False +avg_x_lin_vel = [] +avg_y_lin_vel = [] +avg_yaw_vel = [] +added_frame_info = False +# center_y_pos = None +# center_y_pos = -(pwe.parameters.feet_spacing / 2) +# print(f"center_y_pos: {center_y_pos}") + + +def compute_angular_velocity(quat, prev_quat, dt): + # Convert quaternions to scipy Rotation objects + if prev_quat is None: + prev_quat = quat + r1 = R.from_quat(quat) # Current quaternion + r0 = R.from_quat(prev_quat) # Previous quaternion + + # Compute relative rotation: r_rel = r0^(-1) * r1 + r_rel = r0.inv() * r1 + + # Convert relative rotation to axis-angle + axis, angle = r_rel.as_rotvec(), np.linalg.norm(r_rel.as_rotvec()) + + # Angular velocity (in radians per second) + angular_velocity = axis * (angle / dt) + + return list(angular_velocity) + + +# # convert to linear and angular velocity +def steps_to_vel(step_size, period): + return (step_size * 2) / period + + +while True: + pwe.tick(DT) + if pwe.t <= 0 + args.skip_warmup * 1: + start = pwe.t + last_record = pwe.t + 1 / FPS + last_meshcat_display = pwe.t + 1 / MESHCAT_FPS + continue + + # print(np.around(pwe.robot.get_T_world_fbase()[:3, 3], 3)) + + if pwe.t - last_record >= 1 / FPS: + if args.stand: + T_world_fbase = first_T_world_fbase + else: + T_world_fbase = pwe.robot.get_T_world_fbase() + root_position = list(T_world_fbase[:3, 3]) + # if center_y_pos is None: + # center_y_pos = root_position[1] + + # TODO decomment this line for big duck ? + # root_position[1] = root_position[1] - center_y_pos + if round(root_position[2], 5) < 0: + print(f"BAD root_position: {root_position[2]:.5f}") + root_orientation_quat = list(R.from_matrix(T_world_fbase[:3, :3]).as_quat()) + joints_positions = list(pwe.get_angles().values()) + + if args.stand: + joints_positions = first_joints_positions + T_world_leftFoot = first_T_world_leftFoot + T_world_rightFoot = first_T_world_rightFoot + else: + joints_positions = list(pwe.get_angles().values()) + T_world_leftFoot = pwe.robot.get_T_world_left() + T_world_rightFoot = pwe.robot.get_T_world_right() + + # T_body_leftFoot = ( + # T_world_leftFoot # np.linalg.inv(T_world_fbase) @ T_world_leftFoot + # ) + # T_body_rightFoot = ( + # T_world_rightFoot # np.linalg.inv(T_world_fbase) @ T_world_rightFoot + # ) + + T_body_leftFoot = np.linalg.inv(T_world_fbase) @ T_world_leftFoot + T_body_rightFoot = np.linalg.inv(T_world_fbase) @ T_world_rightFoot + + left_toe_pos = list(T_body_leftFoot[:3, 3]) + right_toe_pos = list(T_body_rightFoot[:3, 3]) + + if not prev_initialized: + prev_root_position = root_position.copy() + prev_root_orientation_euler = ( + R.from_quat(root_orientation_quat).as_euler("xyz").copy() + ) + prev_left_toe_pos = left_toe_pos.copy() + prev_right_toe_pos = right_toe_pos.copy() + prev_joints_positions = joints_positions.copy() + prev_initialized = True + + world_linear_vel = list( + (np.array(root_position) - np.array(prev_root_position)) / (1 / FPS) + ) + avg_x_lin_vel.append(world_linear_vel[0]) + avg_y_lin_vel.append(world_linear_vel[1]) + body_rot_mat = T_world_fbase[:3, :3] + body_linear_vel = list(body_rot_mat.T @ world_linear_vel) + # print("world linear vel", world_linear_vel) + # print("body linear vel", body_linear_vel) + + world_angular_vel = compute_angular_velocity( + root_orientation_quat, prev_root_orientation_quat, (1 / FPS) + ) + + # world_angular_vel = list( + # ( + # R.from_quat(root_orientation_quat).as_euler("xyz") + # - prev_root_orientation_euler + # ) + # / (1 / FPS) + # ) + avg_yaw_vel.append(world_angular_vel[2]) + body_angular_vel = list(body_rot_mat.T @ world_angular_vel) + # print("world angular vel", world_angular_vel) + # print("body angular vel", body_angular_vel) + + if prev_joints_positions == None: + prev_joints_positions = [0] * len(joints_positions) + joints_vel = list( + (np.array(joints_positions) - np.array(prev_joints_positions)) / (1 / FPS) + ) + left_toe_vel = list( + (np.array(left_toe_pos) - np.array(prev_left_toe_pos)) / (1 / FPS) + ) + right_toe_vel = list( + (np.array(right_toe_pos) - np.array(prev_right_toe_pos)) / (1 / FPS) + ) + + foot_contacts = pwe.get_current_support_phase() + + if prev_initialized: + if args.hardware: + episode["Frames"].append( + root_position + + root_orientation_quat + + joints_positions + + left_toe_pos + + right_toe_pos + + world_linear_vel + + world_angular_vel + + joints_vel + + left_toe_vel + + right_toe_vel + + foot_contacts + ) + else: + episode["Frames"].append( + root_position + root_orientation_quat + joints_positions + ) + if args.debug: + episode["Debug_info"].append( + { + "left_foot_pose": list(T_world_leftFoot.flatten()), + "right_foot_pose": list(T_world_rightFoot.flatten()), + } + ) + if not added_frame_info: + added_frame_info = True + offset = 0 + offset_root_pos = offset + offset = offset + len(root_position) + offset_root_quat = offset + offset = offset + len(root_orientation_quat) + offset_joints_pos = offset + offset = offset + len(joints_positions) + offset_left_toe_pos = offset + offset = offset + len(left_toe_pos) + offset_right_toe_pos = offset + offset = offset + len(right_toe_pos) + offset_world_linear_vel = offset + offset = offset + len(world_linear_vel) + offset_world_angular_vel = offset + offset = offset + len(world_angular_vel) + offset_joints_vel = offset + offset = offset + len(joints_vel) + offset_left_toe_vel = offset + offset = offset + len(left_toe_vel) + offset_right_toe_vel = offset + offset = offset + len(right_toe_vel) + offset_foot_contacts = offset + offset = offset + len(foot_contacts) + + episode["Joints"] = list(pwe.get_angles().keys()) + episode["Frame_offset"].append( + { + "root_pos": offset_root_pos, + "root_quat": offset_root_quat, + "joints_pos": offset_joints_pos, + "left_toe_pos": offset_left_toe_pos, + "right_toe_pos": offset_right_toe_pos, + "world_linear_vel": offset_world_linear_vel, + "world_angular_vel": offset_world_angular_vel, + "joints_vel": offset_joints_vel, + "left_toe_vel": offset_left_toe_vel, + "right_toe_vel": offset_right_toe_vel, + "foot_contacts": offset_foot_contacts, + } + ) + episode["Frame_size"].append( + { + "root_pos": len(root_position), + "root_quat": len(root_orientation_quat), + "joints_pos": len(joints_positions), + "left_toe_pos": len(left_toe_pos), + "right_toe_pos": len(right_toe_pos), + "world_linear_vel": len(world_linear_vel), + "world_angular_vel": len(world_angular_vel), + "joints_vel": len(joints_vel), + "left_toe_vel": len(left_toe_vel), + "right_toe_vel": len(right_toe_vel), + "foot_contacts": len(foot_contacts), + } + ) + + prev_root_position = root_position.copy() + prev_root_orientation_quat = root_orientation_quat.copy() + prev_root_orientation_euler = ( + R.from_quat(root_orientation_quat).as_euler("xyz").copy() + ) + prev_left_toe_pos = left_toe_pos.copy() + prev_right_toe_pos = right_toe_pos.copy() + prev_joints_positions = joints_positions.copy() + prev_initialized = True + + last_record = pwe.t + + if DISPLAY_MESHCAT and pwe.t - last_meshcat_display >= 1 / MESHCAT_FPS: + last_meshcat_display = pwe.t + viz.display(pwe.robot.state.q) + footsteps_viz(pwe.trajectory.get_supports()) + robot_frame_viz(pwe.robot, "trunk") + robot_frame_viz(pwe.robot, "left_foot") + robot_frame_viz(pwe.robot, "right_foot") + + # if pwe.t - start > args.length: + # break + if len(episode["Frames"]) == args.length * FPS: + break + + i += 1 + +# skip first 2 seconds to get better average speed +mean_avg_x_lin_vel = np.around(np.mean(avg_x_lin_vel[240:]), 4) +mean_avg_y_lin_vel = np.around(np.mean(avg_y_lin_vel[240:]), 4) +mean_yaw_vel = np.around(np.mean(avg_yaw_vel[240:]), 4) + +print("recorded", len(episode["Frames"]), "frames") +print(f"avg lin_vel_x {mean_avg_x_lin_vel}") +print(f"avg lin_vel_y {mean_avg_y_lin_vel}") +print(f"avg yaw {mean_yaw_vel}") +episode["Vel_x"] = mean_avg_x_lin_vel +episode["Vel_y"] = mean_avg_y_lin_vel +episode["Yaw"] = mean_yaw_vel +episode["Placo"] = { + "dx": args.dx, + "dy": args.dy, + "dtheta": args.dtheta, + "duration": args.length, + "hardware": args.hardware, + "double_support_ratio": pwe.parameters.double_support_ratio, + "startend_double_support_ratio": pwe.parameters.startend_double_support_ratio, + "planned_timesteps": pwe.parameters.planned_timesteps, + "replan_timesteps": pwe.parameters.replan_timesteps, + "walk_com_height": pwe.parameters.walk_com_height, + "walk_foot_height": pwe.parameters.walk_foot_height, + "walk_trunk_pitch": np.rad2deg(pwe.parameters.walk_trunk_pitch), + "walk_foot_rise_ratio": pwe.parameters.walk_foot_rise_ratio, + "single_support_duration": pwe.parameters.single_support_duration, + "single_support_timesteps": pwe.parameters.single_support_timesteps, + "foot_length": pwe.parameters.foot_length, + "feet_spacing": pwe.parameters.feet_spacing, + "zmp_margin": pwe.parameters.zmp_margin, + "foot_zmp_target_x": pwe.parameters.foot_zmp_target_x, + "foot_zmp_target_y": pwe.parameters.foot_zmp_target_y, + "walk_max_dtheta": pwe.parameters.walk_max_dtheta, + "walk_max_dy": pwe.parameters.walk_max_dy, + "walk_max_dx_forward": pwe.parameters.walk_max_dx_forward, + "walk_max_dx_backward": pwe.parameters.walk_max_dx_backward, + "avg_x_lin_vel": mean_avg_x_lin_vel, + "avg_y_lin_vel": mean_avg_y_lin_vel, + "avg_yaw_vel": mean_yaw_vel, + "preset_name": args.preset.split("/")[-1].split(".")[0], + "period": pwe.period, +} + + +x_vel = np.around(steps_to_vel(args.dx, pwe.period), 3) +y_vel = np.around(steps_to_vel(args.dy, pwe.period), 3) +theta_vel = np.around(steps_to_vel(args.dtheta, pwe.period), 3) + +print(f"computed xvel: {x_vel}, mean xvel: {mean_avg_x_lin_vel}") +print(f"computed yvel: {y_vel}, mean yvel: {mean_avg_y_lin_vel}") +print(f"computed thetavel: {theta_vel}, mean thetavel: {mean_yaw_vel}") + + +name = f"{args.name}_{x_vel}_{y_vel}_{theta_vel}" # Do we need the id in the name ? +# name = f"{x_vel}_{y_vel}_{theta_vel}" +file_name = name + str(".json") +file_path = os.path.join(args.output_dir, file_name) +os.makedirs(args.output_dir, exist_ok=True) +print("DONE, saving", file_name) +with open(file_path, "w") as f: + json.encoder.c_make_encoder = None + json.encoder.float = RoundingFloat + json.dump(episode, f, indent=4) diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/gait_playground.py b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/gait_playground.py new file mode 100644 index 0000000..cff4534 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/gait_playground.py @@ -0,0 +1,612 @@ +import argparse +from flask import Flask, render_template, request, redirect, url_for, jsonify +import numpy as np +import webbrowser +import threading +import json +import os +import time +from os.path import join +from threading import current_thread + +import numpy as np +import placo +from placo_utils.visualization import footsteps_viz, robot_frame_viz, robot_viz, get_viewer +from scipy.spatial.transform import Rotation as R + +from placo_walk_engine import PlacoWalkEngine + +parser = argparse.ArgumentParser() +parser.add_argument("--dx", type=float, default=0) +parser.add_argument("--dy", type=float, default=0) +parser.add_argument("--output_dir", type=str, default=".") +parser.add_argument("--dtheta", type=float, default=0) +parser.add_argument("--double_support_ratio", type=float, default=None) +parser.add_argument("--startend_double_support_ratio", type=float, default=None) +parser.add_argument("--planned_timesteps", type=float, default=None) +parser.add_argument("--replan_timesteps", type=float, default=None) +parser.add_argument("--walk_com_height", type=float, default=None) +parser.add_argument("--walk_foot_height", type=float, default=None) +parser.add_argument("--walk_trunk_pitch", type=float, default=None) +parser.add_argument("--walk_foot_rise_ratio", type=float, default=None) +parser.add_argument("--single_support_duration", type=float, default=None) +parser.add_argument("--single_support_timesteps", type=float, default=None) +parser.add_argument("--foot_length", type=float, default=None) +parser.add_argument("--feet_spacing", type=float, default=None) +parser.add_argument("--zmp_margin", type=float, default=None) +parser.add_argument("--foot_zmp_target_x", type=float, default=None) +parser.add_argument("--foot_zmp_target_y", type=float, default=None) +parser.add_argument("--walk_max_dtheta", type=float, default=None) +parser.add_argument("--walk_max_dy", type=float, default=None) +parser.add_argument("--walk_max_dx_forward", type=float, default=None) +parser.add_argument("--walk_max_dx_backward", type=float, default=None) +parser.add_argument("-l", "--length", type=int, default=10) +parser.add_argument( + "--duck", + choices=["go_bdx", "open_duck_mini", "open_duck_mini_v2"], + help="Duck type", + required=True, +) +parser.add_argument("--preset", type=str, help="Path to the preset file") +args = parser.parse_args() + +app = Flask(__name__) + +FPS = 60 +MESHCAT_FPS = 60 +DT = 0.001 + +episode = { + "LoopMode": "Wrap", + "FrameDuration": np.around(1 / FPS, 4), + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Debug_info": [], + "Frames": [], + "MotionWeight": 1, +} + +def open_browser(): + try: + webbrowser.open_new('http://127.0.0.1:5000/') + except: + print("Failed to open the default browser. Trying Google Chrome.") + try: + webbrowser.get('google-chrome').open_new('http://127.0.0.1:5000/') + except: + print("Failed to open Google Chrome. Make sure it's installed and accessible.") + +script_path = os.path.dirname(os.path.abspath(__file__)) +# Define the parameters class to hold the variables +class GaitParameters: + def __init__(self): + if args.duck == "open_duck_mini": + self.robot = 'open_duck_mini' + self.robot_urdf = "open_duck_mini.urdf" + self.asset_path = os.path.join(script_path, "../open_duck_reference_motion_generator/robots/open_duck_mini/") + elif args.duck == "open_duck_mini_v2": + self.robot = 'open_duck_mini_v2' + self.robot_urdf = "open_duck_mini_v2.urdf" + self.asset_path = os.path.join(script_path, "../open_duck_reference_motion_generator/robots/open_duck_mini_v2/") + elif args.duck == "go_bdx": + self.robot = 'go_bdx' + self.robot_urdf = "go_bdx.urdf" + self.asset_path = os.path.join(script_path, "../open_duck_reference_motion_generator/robots/go_bdx/") + self.dx = 0.1 + self.dy = 0.0 + self.dtheta = 0.0 + self.duration = 5 + self.hardware = True + + def reset(self, pwe): + pwe.parameters.double_support_ratio = self.double_support_ratio + pwe.parameters.startend_double_support_ratio = self.startend_double_support_ratio + pwe.parameters.planned_timesteps = self.planned_timesteps + pwe.parameters.replan_timesteps = self.replan_timesteps + pwe.parameters.walk_com_height = self.walk_com_height + pwe.parameters.walk_foot_height = self.walk_foot_height + pwe.parameters.walk_trunk_pitch = np.deg2rad(self.walk_trunk_pitch) + pwe.parameters.walk_foot_rise_ratio = self.walk_foot_rise_ratio + pwe.parameters.single_support_duration = self.single_support_duration + pwe.parameters.single_support_timesteps = self.single_support_timesteps + pwe.parameters.foot_length = self.foot_length + pwe.parameters.feet_spacing = self.feet_spacing + pwe.parameters.zmp_margin = self.zmp_margin + pwe.parameters.foot_zmp_target_x = self.foot_zmp_target_x + pwe.parameters.foot_zmp_target_y = self.foot_zmp_target_y + pwe.parameters.walk_max_dtheta = self.walk_max_dtheta + pwe.parameters.walk_max_dy = self.walk_max_dy + pwe.parameters.walk_max_dx_forward = self.walk_max_dx_forward + pwe.parameters.walk_max_dx_backward = self.walk_max_dx_backward + + def save_to_json(self, filename): + data = { + 'dx': self.dx, + 'dy': self.dy, + 'dtheta': self.dtheta, + 'duration': self.duration, + 'hardware': self.hardware, + 'double_support_ratio': self.double_support_ratio, + 'startend_double_support_ratio': self.startend_double_support_ratio, + 'planned_timesteps': self.planned_timesteps, + 'replan_timesteps': self.replan_timesteps, + 'walk_com_height': self.walk_com_height, + 'walk_foot_height': self.walk_foot_height, + 'walk_trunk_pitch': self.walk_trunk_pitch, + 'walk_foot_rise_ratio': self.walk_foot_rise_ratio, + 'single_support_duration': self.single_support_duration, + 'single_support_timesteps': self.single_support_timesteps, + 'foot_length': self.foot_length, + 'feet_spacing': self.feet_spacing, + 'zmp_margin': self.zmp_margin, + 'foot_zmp_target_x': self.foot_zmp_target_x, + 'foot_zmp_target_y': self.foot_zmp_target_y, + 'walk_max_dtheta': self.walk_max_dtheta, + 'walk_max_dy': self.walk_max_dy, + 'walk_max_dx_forward': self.walk_max_dx_forward, + 'walk_max_dx_backward': self.walk_max_dx_backward, + } + with open(filename, 'w') as f: + json.dump(data, f, indent=4) + + def create_pwe(self, parameters=None): + pwe = PlacoWalkEngine(self.asset_path, self.robot_urdf, parameters) + self.reset(pwe) + pwe.set_traj(0, 0, 0) + return pwe + + def custom_preset_name(self): + return f"placo_{gait.robot}_defaults.json" + + def save_custom_presets(self): + filename = self.custom_preset_name() + self.save_to_json(filename) + + def load_defaults(self, pwe): + self.load_from_json(os.path.join(pwe.asset_path, "placo_defaults.json")) + + def load_from_json(self, filename): + with open(filename, 'r') as f: + data = json.load(f) + self.load_from_data(data) + + def load_from_data(self, data): + self.dx = data.get('dx') + self.dy = data.get('dy') + self.dtheta = data.get('dtheta') + self.duration = data.get('duration') + self.hardware = data.get('hardware') + self.double_support_ratio = data.get('double_support_ratio') + self.startend_double_support_ratio = data.get('startend_double_support_ratio') + self.planned_timesteps = data.get('planned_timesteps') + self.replan_timesteps = data.get('replan_timesteps') + self.walk_com_height = data.get('walk_com_height') + self.walk_foot_height = data.get('walk_foot_height') + self.walk_trunk_pitch = data.get('walk_trunk_pitch') + self.walk_foot_rise_ratio = data.get('walk_foot_rise_ratio') + self.single_support_duration = data.get('single_support_duration') + self.single_support_timesteps = data.get('single_support_timesteps') + self.foot_length = data.get('foot_length') + self.feet_spacing = data.get('feet_spacing') + self.zmp_margin = data.get('zmp_margin') + self.foot_zmp_target_x = data.get('foot_zmp_target_x') + self.foot_zmp_target_y = data.get('foot_zmp_target_y') + self.walk_max_dtheta = data.get('walk_max_dtheta') + self.walk_max_dy = data.get('walk_max_dy') + self.walk_max_dx_forward = data.get('walk_max_dx_forward') + self.walk_max_dx_backward = data.get('walk_max_dx_backward') + +if __name__ == '__main__': + print('exit') + +gait = GaitParameters() +if args.preset: + filename = args.preset +else: + filename = gait.custom_preset_name() + if not os.path.exists(filename): + filename = os.path.join(gait.asset_path, "placo_defaults.json") +with open(filename, 'r') as f: + gait_parameters = json.load(f) + print(f"gait_parameters {gait_parameters}") + if args.double_support_ratio is not None: + gait_parameters['double_support_ratio'] = args.double_support_ratio + if args.startend_double_support_ratio is not None: + gait_parameters['startend_double_support_ratio'] = args.startend_double_support_ratio + if args.planned_timesteps is not None: + gait_parameters['planned_timesteps'] = args.planned_timesteps + if args.replan_timesteps is not None: + gait_parameters['replan_timesteps'] = args.replan_timesteps + if args.walk_com_height is not None: + gait_parameters['walk_com_height'] = args.walk_com_height + if args.walk_foot_height is not None: + gait_parameters['walk_foot_height'] = args.walk_foot_height + if args.walk_trunk_pitch is not None: + gait_parameters['walk_trunk_pitch'] = np.deg2rad(args.walk_trunk_pitch) + if args.walk_foot_rise_ratio is not None: + gait_parameters['walk_foot_rise_ratio'] = args.walk_foot_rise_ratio + if args.single_support_duration is not None: + gait_parameters['single_support_duration'] = args.single_support_duration + if args.single_support_timesteps is not None: + gait_parameters['single_support_timesteps'] = args.single_support_timesteps + if args.foot_length is not None: + gait_parameters['foot_length'] = args.foot_length + if args.feet_spacing is not None: + gait_parameters['feet_spacing'] = args.feet_spacing + if args.zmp_margin is not None: + gait_parameters['zmp_margin'] = args.zmp_margin + if args.foot_zmp_target_x is not None: + gait_parameters['foot_zmp_target_x'] = args.foot_zmp_target_x + if args.foot_zmp_target_y is not None: + gait_parameters['foot_zmp_target_y'] = args.foot_zmp_target_y + if args.walk_max_dtheta is not None: + gait_parameters['walk_max_dtheta'] = args.walk_max_dtheta + if args.walk_max_dy is not None: + gait_parameters['walk_max_dy'] = args.walk_max_dy + if args.walk_max_dx_forward is not None: + gait_parameters['walk_max_dx_forward'] = args.walk_max_dx_forward + if args.walk_max_dx_backward is not None: + gait_parameters['walk_max_dx_backward'] = args.walk_max_dx_backward + gait.load_from_data(gait_parameters) +pwe = gait.create_pwe(gait_parameters) +viz = robot_viz(pwe.robot) +viz.display(pwe.robot.state.q) + +threading.Timer(1, open_browser).start() + +run_loop = False +dorun = False +doreset = False +doupdate = False +gait_condition = threading.Condition() +# gait_start_semaphore = threading.Semaphore(0) + +@app.route('/log', methods=['POST']) +def log_message(): + message = request.json.get('message', '') + print(f"Placo: {message}") + return 'Logged', 200 + +@app.route('/', methods=['GET', 'POST']) +def index(): + global run_loop + return render_template('index.html', parameters=gait) + +@app.route('/save_state', methods=['POST']) +def save_state(): + gait.save_custom_presets() + return "", 200 + +@app.route('/defaults', methods=['GET']) +def defaults(): + if os.path.exists("bdx_state.json"): + gait.load_from_json() + else: + gait.load_defaults(pwe) + parameters = gait.__dict__ + return jsonify(parameters) + +@app.route('/get', methods=['GET']) +def get_parameters(): + # Convert gait parameters to a dictionary + parameters = gait.__dict__ + return jsonify(parameters) + +@app.route('/set_playback_speed', methods=['POST']) +def set_playback_speed(): + global DT + data = request.get_json() + speed = data.get('speed') + if speed == 0.25: + DT = 0.00001 + return "Speed changed successfully", 200 + elif speed == 0.5: + DT = 0.0001 + return "Speed changed successfully", 200 + elif speed == 1.0: + DT = 0.001 + return "Speed changed successfully", 200 + return "Invalid speed selection", 400 + +@app.route('/change_robot', methods=['POST']) +def change_robot(): + global run_loop + global doreset + global dorun + data = request.get_json() + selected_robot = data.get('robot') + print(f"selected_robot: {selected_robot}") + if selected_robot in ['go_bdx', 'open_duck_mini']: + if selected_robot != gait.robot: + gait.robot = selected_robot + # Reset the gait generator to use the new robot + with gait_condition: + run_loop = False + doreset = True + dorun = False + gait_condition.notify() + return "Robot changed successfully", 200 + else: + return "Invalid robot selection", 400 + +@app.route('/run', methods=['POST']) +def run(): + global dorun + global run_loop + # Update parameters from sliders + gait.dx = float(request.form['dx']) + gait.dy = float(request.form['dy']) + gait.dtheta = float(request.form['dtheta']) + gait.duration = int(request.form['duration']) + gait.double_support_ratio = float(request.form['double_support_ratio']) + gait.startend_double_support_ratio = float(request.form['startend_double_support_ratio']) + gait.planned_timesteps = int(request.form['planned_timesteps']) + gait.replan_timesteps = int(request.form['replan_timesteps']) + gait.walk_com_height = float(request.form['walk_com_height']) + gait.walk_foot_height = float(request.form['walk_foot_height']) + gait.walk_trunk_pitch = int(request.form['walk_trunk_pitch']) # Degrees + gait.walk_foot_rise_ratio = float(request.form['walk_foot_rise_ratio']) + gait.single_support_duration = float(request.form['single_support_duration']) + gait.single_support_timesteps = int(request.form['single_support_timesteps']) + gait.foot_length = float(request.form['foot_length']) + gait.feet_spacing = float(request.form['feet_spacing']) + gait.zmp_margin = float(request.form['zmp_margin']) + gait.foot_zmp_target_x = float(request.form['foot_zmp_target_x']) + gait.foot_zmp_target_y = float(request.form['foot_zmp_target_y']) + gait.walk_max_dtheta = float(request.form['walk_max_dtheta']) + gait.walk_max_dy = float(request.form['walk_max_dy']) + gait.walk_max_dx_forward = float(request.form['walk_max_dx_forward']) + gait.walk_max_dx_backward = float(request.form['walk_max_dx_backward']) + with gait_condition: + run_loop = False + dorun = True + gait_condition.notify() + return "", 200 + +@app.route('/update', methods=['POST']) +def update(): + global dorun + global run_loop + # Update parameters from sliders + gait.dx = float(request.form['dx']) + gait.dy = float(request.form['dy']) + gait.dtheta = float(request.form['dtheta']) + gait.duration = int(request.form['duration']) + gait.double_support_ratio = float(request.form['double_support_ratio']) + gait.startend_double_support_ratio = float(request.form['startend_double_support_ratio']) + gait.planned_timesteps = int(request.form['planned_timesteps']) + gait.replan_timesteps = int(request.form['replan_timesteps']) + gait.walk_com_height = float(request.form['walk_com_height']) + gait.walk_foot_height = float(request.form['walk_foot_height']) + gait.walk_trunk_pitch = int(request.form['walk_trunk_pitch']) # Degrees + gait.walk_foot_rise_ratio = float(request.form['walk_foot_rise_ratio']) + gait.single_support_duration = float(request.form['single_support_duration']) + gait.single_support_timesteps = int(request.form['single_support_timesteps']) + gait.foot_length = float(request.form['foot_length']) + gait.feet_spacing = float(request.form['feet_spacing']) + gait.zmp_margin = float(request.form['zmp_margin']) + gait.foot_zmp_target_x = float(request.form['foot_zmp_target_x']) + gait.foot_zmp_target_y = float(request.form['foot_zmp_target_y']) + gait.walk_max_dtheta = float(request.form['walk_max_dtheta']) + gait.walk_max_dy = float(request.form['walk_max_dy']) + gait.walk_max_dx_forward = float(request.form['walk_max_dx_forward']) + gait.walk_max_dx_backward = float(request.form['walk_max_dx_backward']) + with gait_condition: + dorun = run_loop + run_loop = False + gait_condition.notify() + return "", 200 + +@app.route('/stop', methods=['POST']) +def stop(): + global run_loop + global dorun + dorun = False + run_loop = False + print("Stopping") + return "Loop stopped successfully", 200 + +@app.route('/reset', methods=['POST']) +def reset(): + global run_loop + global doreset + with gait_condition: + run_loop = False + doreset = True + gait_condition.notify() + print("Resetting") + return "Loop stopped successfully", 200 + +def gait_generator_thread(): + global run_loop + global doreset + global dorun + global pwe + global viz + global DT + while True: + print("gait generator waiting") + with gait_condition: + if doreset: + pwe = gait.create_pwe() + viz = robot_viz(pwe.robot) + viz.display(pwe.robot.state.q) + footsteps_viz(pwe.trajectory.get_supports()) + robot_frame_viz(pwe.robot, "trunk") + robot_frame_viz(pwe.robot, "left_foot") + robot_frame_viz(pwe.robot, "right_foot") + doreset = False + continue + if not dorun: + gait_condition.wait() + dorun = False + if doreset: + print("RESETTING") + pwe = gait.create_pwe() + viz = robot_viz(pwe.robot) + viz.display(pwe.robot.state.q) + footsteps_viz(pwe.trajectory.get_supports()) + robot_frame_viz(pwe.robot, "trunk") + robot_frame_viz(pwe.robot, "left_foot") + robot_frame_viz(pwe.robot, "right_foot") + doreset = False + continue + run_loop = True + gait.reset(pwe) + pwe.set_traj(gait.dx, gait.dy, gait.dtheta + 0.001) + start = pwe.t + + last_record = 0 + last_meshcat_display = 0 + prev_root_position = [0, 0, 0] + prev_root_orientation_euler = [0, 0, 0] + prev_left_toe_pos = [0, 0, 0] + prev_right_toe_pos = [0, 0, 0] + prev_joints_positions = None + i = 0 + prev_initialized = False + while run_loop: + pwe.tick(DT) + if pwe.t <= 0: + # print("waiting ") + start = pwe.t + last_record = pwe.t + 1 / FPS + last_meshcat_display = pwe.t + 1 / MESHCAT_FPS + continue + + # print(np.around(pwe.robot.get_T_world_fbase()[:3, 3], 3)) + + if pwe.t - last_record >= 1 / FPS: + # before + # T_world_fbase = pwe.robot.get_T_world_fbase() + # after + T_world_fbase = pwe.robot.get_T_world_trunk() + # fv.pushFrame(T_world_fbase, "trunk") + root_position = list(T_world_fbase[:3, 3]) + root_orientation_quat = list(R.from_matrix(T_world_fbase[:3, :3]).as_quat()) + joints_positions = list(pwe.get_angles().values()) + + T_world_leftFoot = pwe.robot.get_T_world_left() + T_world_rightFoot = pwe.robot.get_T_world_right() + + # fv.pushFrame(T_world_leftFoot, "left") + # fv.pushFrame(T_world_rightFoot, "right") + + T_body_leftFoot = np.linalg.inv(T_world_fbase) @ T_world_leftFoot + T_body_rightFoot = np.linalg.inv(T_world_fbase) @ T_world_rightFoot + + # left_foot_pose = pwe.robot.get_T_world_left() + # right_foot_pose = pwe.robot.get_T_world_right() + + left_toe_pos = list(T_body_leftFoot[:3, 3]) + right_toe_pos = list(T_body_rightFoot[:3, 3]) + + world_linear_vel = list( + (np.array(root_position) - np.array(prev_root_position)) / (1 / FPS) + ) + body_rot_mat = T_world_fbase[:3, :3] + body_linear_vel = list(body_rot_mat.T @ world_linear_vel) + # print("world linear vel", world_linear_vel) + # print("body linear vel", body_linear_vel) + + world_angular_vel = list( + ( + R.from_quat(root_orientation_quat).as_euler("xyz") + - prev_root_orientation_euler + ) + / (1 / FPS) + ) + body_angular_vel = list(body_rot_mat.T @ world_angular_vel) + # print("world angular vel", world_angular_vel) + # print("body angular vel", body_angular_vel) + + if prev_joints_positions == None: + prev_joints_positions = [0] * len(joints_positions) + + joints_vel = list( + (np.array(joints_positions) - np.array(prev_joints_positions)) / (1 / FPS) + ) + left_toe_vel = list( + (np.array(left_toe_pos) - np.array(prev_left_toe_pos)) / (1 / FPS) + ) + right_toe_vel = list( + (np.array(right_toe_pos) - np.array(prev_right_toe_pos)) / (1 / FPS) + ) + + foot_contacts = pwe.get_current_support_phase() + + if prev_initialized: + if gait.hardware: + episode["Frames"].append( + root_position + + root_orientation_quat + + joints_positions + + left_toe_pos + + right_toe_pos + + world_linear_vel + + world_angular_vel + + joints_vel + + left_toe_vel + + right_toe_vel + + foot_contacts + ) + else: + episode["Frames"].append( + root_position + root_orientation_quat + joints_positions + ) + episode["Debug_info"].append( + { + "left_foot_pose": list(T_world_leftFoot.flatten()), + "right_foot_pose": list(T_world_rightFoot.flatten()), + } + ) + + prev_root_position = root_position.copy() + prev_root_orientation_euler = ( + R.from_quat(root_orientation_quat).as_euler("xyz").copy() + ) + prev_left_toe_pos = left_toe_pos.copy() + prev_right_toe_pos = right_toe_pos.copy() + prev_joints_positions = joints_positions.copy() + prev_initialized = True + + last_record = pwe.t + # print("saved frame") + + if pwe.t - last_meshcat_display >= 1 / MESHCAT_FPS: + last_meshcat_display = pwe.t + viz.display(pwe.robot.state.q) + footsteps_viz(pwe.trajectory.get_supports()) + robot_frame_viz(pwe.robot, "trunk") + robot_frame_viz(pwe.robot, "left_foot") + robot_frame_viz(pwe.robot, "right_foot") + + if pwe.t - start > gait.duration: + break + + i += 1 + run_loop = False + # print("recorded", len(episode["Frames"]), "frames") + # args_name = "dummy" + # file_name = args_name + str(".txt") + # file_path = os.path.join(args.output_dir, file_name) + # os.makedirs(args.output_dir, exist_ok=True) + # print("DONE, saving", file_name) + # with open(file_path, "w") as f: + # json.dump(episode, f) + +def open_browser(): + try: + webbrowser.open_new('http://127.0.0.1:5000/') + except: + print("Failed to open the default browser. Trying Google Chrome.") + try: + webbrowser.get('google-chrome').open_new('http://127.0.0.1:5000/') + except: + print("Failed to open Google Chrome. Make sure it's installed and accessible.") + +thread = threading.Thread(target=gait_generator_thread, daemon=True) +thread.start() + +if __name__ == '__main__': + app.run(debug=False) diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/placo_walk_engine.py b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/placo_walk_engine.py new file mode 100644 index 0000000..9f16b9d --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/placo_walk_engine.py @@ -0,0 +1,287 @@ +import time +import warnings +import json + +import numpy as np +import placo +import os + +warnings.filterwarnings("ignore") + +DT = 0.01 +REFINE = 10 + + +class PlacoWalkEngine: + def __init__( + self, + asset_path: str = "", + model_filename: str = "go_bdx.urdf", + init_params: dict = {}, + ignore_feet_contact: bool = False, + knee_limits: list = None, + ) -> None: + model_filename = os.path.join(asset_path, model_filename) + self.asset_path = asset_path + self.model_filename = model_filename + self.ignore_feet_contact = ignore_feet_contact + + robot_type = asset_path.split("/")[-1] + if robot_type in ["open_duck_mini", "go_bdx"]: + knee_limits = knee_limits or [-0.2, -0.01] + else: + knee_limits = knee_limits or [0.2, 0.01] + + # Loading the robot + self.robot = placo.HumanoidRobot(model_filename) + + self.parameters = placo.HumanoidParameters() + if init_params is not None: + self.load_parameters(init_params) + else: + defaults_filename = os.path.join(asset_path, "placo_defaults.json") + self.load_defaults(defaults_filename) + + # Creating the kinematics solver + self.solver = placo.KinematicsSolver(self.robot) + self.solver.enable_velocity_limits(True) + self.robot.set_velocity_limits(12.0) + self.solver.enable_joint_limits(False) + self.solver.dt = DT / REFINE + + self.robot.set_joint_limits("left_knee", *knee_limits) + self.robot.set_joint_limits("right_knee", *knee_limits) + + # Creating the walk QP tasks + self.tasks = placo.WalkTasks() + if hasattr(self.parameters, 'trunk_mode'): + self.tasks.trunk_mode = self.parameters.trunk_mode + self.tasks.com_x = 0.0 + self.tasks.initialize_tasks(self.solver, self.robot) + self.tasks.left_foot_task.orientation().mask.set_axises("yz", "local") + self.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 + self.joints = self.parameters.joints + joint_degrees = self.parameters.joint_angles + joint_radians = {joint: np.deg2rad(degrees) for joint, degrees in joint_degrees.items()} + self.joints_task = self.solver.add_joints_task() + self.joints_task.set_joints(joint_radians) + self.joints_task.configure("joints", "soft", 1.0) + + # Placing the robot in the initial position + print("Placing the robot in the initial position...") + self.tasks.reach_initial_pose( + np.eye(4), + self.parameters.feet_spacing, + self.parameters.walk_com_height, + self.parameters.walk_trunk_pitch, + ) + print("Initial position reached") + + print(self.get_angles()) + # exit() + + # Creating the FootstepsPlanner + self.repetitive_footsteps_planner = placo.FootstepsPlannerRepetitive( + self.parameters + ) + self.d_x = 0.0 + self.d_y = 0.0 + self.d_theta = 0.0 + self.nb_steps = 5 + self.repetitive_footsteps_planner.configure( + self.d_x, self.d_y, self.d_theta, self.nb_steps + ) + + # Planning footsteps + self.T_world_left = placo.flatten_on_floor(self.robot.get_T_world_left()) + self.T_world_right = placo.flatten_on_floor(self.robot.get_T_world_right()) + self.footsteps = self.repetitive_footsteps_planner.plan( + placo.HumanoidRobot_Side.left, self.T_world_left, self.T_world_right + ) + + self.supports = placo.FootstepsPlanner.make_supports( + self.footsteps, True, self.parameters.has_double_support(), True + ) + + # Creating the pattern generator and making an initial plan + self.walk = placo.WalkPatternGenerator(self.robot, self.parameters) + self.trajectory = self.walk.plan(self.supports, self.robot.com_world(), 0.0) + + self.time_since_last_right_contact = 0.0 + self.time_since_last_left_contact = 0.0 + self.start = None + self.initial_delay = -1.0 + # self.initial_delay = 0 + self.t = self.initial_delay + self.last_replan = 0 + + # TODO remove startend_double_support_duration() when starting and ending ? + self.period = ( + 2 * self.parameters.single_support_duration + + 2 * self.parameters.double_support_duration() + ) + print("## period:", self.period) + + def load_defaults(self, filename): + with open(filename, 'r') as f: + data = json.load(f) + params = self.parameters + self.load_parameters(data) + + def load_parameters(self, data): + params = self.parameters + params.double_support_ratio = data.get('double_support_ratio', params.double_support_ratio) + params.startend_double_support_ratio = data.get('startend_double_support_ratio', params.startend_double_support_ratio) + params.planned_timesteps = data.get('planned_timesteps', params.planned_timesteps) + params.replan_timesteps = data.get('replan_timesteps', params.replan_timesteps) + params.walk_com_height = data.get('walk_com_height', params.walk_com_height) + params.walk_foot_height = data.get('walk_foot_height', params.walk_foot_height) + params.walk_trunk_pitch = np.deg2rad(data.get('walk_trunk_pitch', np.rad2deg(params.walk_trunk_pitch))) + params.walk_foot_rise_ratio = data.get('walk_foot_rise_ratio', params.walk_foot_rise_ratio) + params.single_support_duration = data.get('single_support_duration', params.single_support_duration) + params.single_support_timesteps = data.get('single_support_timesteps', params.single_support_timesteps) + params.foot_length = data.get('foot_length', params.foot_length) + params.feet_spacing = data.get('feet_spacing', params.feet_spacing) + params.zmp_margin = data.get('zmp_margin', params.zmp_margin) + params.foot_zmp_target_x = data.get('foot_zmp_target_x', params.foot_zmp_target_x) + params.foot_zmp_target_y = data.get('foot_zmp_target_y', params.foot_zmp_target_y) + params.walk_max_dtheta = data.get('walk_max_dtheta', params.walk_max_dtheta) + params.walk_max_dy = data.get('walk_max_dy', params.walk_max_dy) + params.walk_max_dx_forward = data.get('walk_max_dx_forward', params.walk_max_dx_forward) + params.walk_max_dx_backward = data.get('walk_max_dx_backward', params.walk_max_dx_backward) + params.joints = data.get('joints', []) + params.joint_angles = data.get('joint_angles', []) + if 'trunk_mode' in data: + params.trunk_mode = data.get('trunk_mode') + + def get_angles(self): + angles = {joint: self.robot.get_joint(joint) for joint in self.joints} + return angles + + def reset(self): + self.t = 0 + self.start = None + self.last_replan = 0 + self.time_since_last_right_contact = 0.0 + self.time_since_last_left_contact = 0.0 + + self.tasks.reach_initial_pose( + np.eye(4), + self.parameters.feet_spacing, + self.parameters.walk_com_height, + self.parameters.walk_trunk_pitch, + ) + + # Planning footsteps + self.T_world_left = placo.flatten_on_floor(self.robot.get_T_world_left()) + self.T_world_right = placo.flatten_on_floor(self.robot.get_T_world_right()) + self.footsteps = self.repetitive_footsteps_planner.plan( + placo.HumanoidRobot_Side.left, self.T_world_left, self.T_world_right + ) + + self.supports = placo.FootstepsPlanner.make_supports( + self.footsteps, True, self.parameters.has_double_support(), True + ) + self.trajectory = self.walk.plan(self.supports, self.robot.com_world(), 0.0) + + def set_traj(self, d_x, d_y, d_theta): + self.d_x = d_x + self.d_y = d_y + self.d_theta = d_theta + self.repetitive_footsteps_planner.configure( + self.d_x, self.d_y, self.d_theta, self.nb_steps + ) + + def get_footsteps_in_world(self): + footsteps = self.trajectory.get_supports() + footsteps_in_world = [] + for footstep in footsteps: + if not footstep.is_both(): + footsteps_in_world.append(footstep.frame()) + + for i in range(len(footsteps_in_world)): + footsteps_in_world[i][:3, 3][1] += self.parameters.feet_spacing / 2 + + return footsteps_in_world + + def get_footsteps_in_robot_frame(self): + T_world_fbase = self.robot.get_T_world_fbase() + + footsteps = self.trajectory.get_supports() + footsteps_in_robot_frame = [] + for footstep in footsteps: + if not footstep.is_both(): + T_world_footstepFrame = footstep.frame().copy() + T_fbase_footstepFrame = ( + np.linalg.inv(T_world_fbase) @ T_world_footstepFrame + ) + T_fbase_footstepFrame = placo.flatten_on_floor(T_fbase_footstepFrame) + T_fbase_footstepFrame[:3, 3][2] = -T_world_fbase[:3, 3][2] + + footsteps_in_robot_frame.append(T_fbase_footstepFrame) + + return footsteps_in_robot_frame + + def get_current_support_phase(self): + if self.trajectory.support_is_both(self.t): + return [1, 1] + elif str(self.trajectory.support_side(self.t)) == "left": + return [1, 0] + elif str(self.trajectory.support_side(self.t)) == "right": + return [0, 1] + else: + raise AssertionError(f"Invalid phase: {self.trajectory.support_side(self.t)}") + + def tick(self, dt, left_contact=True, right_contact=True): + if self.start is None: + self.start = time.time() + + if not self.ignore_feet_contact: + if left_contact: + self.time_since_last_left_contact = 0.0 + if right_contact: + self.time_since_last_right_contact = 0.0 + + falling = not self.ignore_feet_contact and ( + self.time_since_last_left_contact > self.parameters.single_support_duration + or self.time_since_last_right_contact + > self.parameters.single_support_duration + ) + + for k in range(REFINE): + # Updating the QP tasks from planned trajectory + if not falling: + self.tasks.update_tasks_from_trajectory( + self.trajectory, self.t - dt + k * dt / REFINE + ) + + self.robot.update_kinematics() + _ = self.solver.solve(True) + + # If enough time elapsed and we can replan, do the replanning + if ( + self.t - self.last_replan + > self.parameters.replan_timesteps * self.parameters.dt() + and self.walk.can_replan_supports(self.trajectory, self.t) + ): + self.last_replan = self.t + + # Replanning footsteps from current trajectory + self.supports = self.walk.replan_supports( + self.repetitive_footsteps_planner, self.trajectory, self.t + ) + + # Replanning CoM trajectory, yielding a new trajectory we can switch to + self.trajectory = self.walk.replan(self.supports, self.trajectory, self.t) + + self.time_since_last_left_contact += dt + self.time_since_last_right_contact += dt + self.t += dt + + # while time.time() < self.start_t + self.t: + # time.sleep(1e-3) diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/ear_left.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/ear_left.stl new file mode 100644 index 0000000..32b21dc --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/ear_left.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c1347e88747975ffa8458c47e64151534e45828fe89c0d06565c678e2e51afac +size 90684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/ear_right.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/ear_right.stl new file mode 100644 index 0000000..d422b41 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/ear_right.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:11b83391f286f4a08c25e25104aff7261f500aa32d3161a1c3cf661245a5e9d4 +size 90684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/head_body_pitch.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/head_body_pitch.stl new file mode 100644 index 0000000..e4cc16a --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/head_body_pitch.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3e50310186a71fc9f658a64865b17da15fd4c47095740348d0e55c10ba1ac1e1 +size 377584 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/head_body_roll.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/head_body_roll.stl new file mode 100644 index 0000000..01f9e31 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/head_body_roll.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7315dd2b1e532697b25608e7fdfb27815aa1a371f408fbe4f52f1acfebbc90c1 +size 2180084 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/head_body_yaw.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/head_body_yaw.stl new file mode 100644 index 0000000..d18f0e3 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/head_body_yaw.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e03d3acc1c7f8760bb3f92c6db91c8439f90b179a3f107e5abe6b5b6a5d3d32b +size 1205884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_foot.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_foot.stl new file mode 100644 index 0000000..2b98f92 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_foot.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ac3c9b15ab09ef4b8d6c7a8da9e22223453e1dc6328d1f49b9c8d3618bf7c480 +size 84684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_hip.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_hip.stl new file mode 100644 index 0000000..5bc8ebc --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_hip.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d1dad2f89c3f90607786363bebdc9cf03dde4ad2873fc473d2ca0b4c87fd9225 +size 160984 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_hip_rotation.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_hip_rotation.stl new file mode 100644 index 0000000..395a12e --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_hip_rotation.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c088a5ed10033c65d5aae9f28faa7f2e8bce96cca3aa65a30888588a448174fa +size 342784 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_shin.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_shin.stl new file mode 100644 index 0000000..4de8b25 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_shin.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a20d0e1097f78aea8df96c9afb0b2bae06d482e4380ded55d0a3b8f4c586943e +size 272484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_thigh.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_thigh.stl new file mode 100644 index 0000000..de1d569 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/left_thigh.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c70799d3fbdd08d908f7504b5f55990e1c93697a689993ae670c8c86a5a70982 +size 362184 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/neck.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/neck.stl new file mode 100644 index 0000000..36b0aee --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/neck.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d2903e637dd4684debc299b728cc7d55680bb6d2cc3082aec7ac4037ba0a04dc +size 870884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/pelvis.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/pelvis.stl new file mode 100644 index 0000000..35c36c1 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/pelvis.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:791a6545535e9a33880cbfd4f87d18c47cc26c6360676b6b6b018b71d86d82a5 +size 1901684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_foot.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_foot.stl new file mode 100644 index 0000000..5fb6f53 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_foot.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:78954ce7075bb98cf1a71cd482ccc764776e609cfddf98fac2ca5073a5e60e4e +size 84684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_hip.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_hip.stl new file mode 100644 index 0000000..3750754 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_hip.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c5a38cce93f7a7733f5154cafd823fd2f53d9b77429a8ca50223a6b9578e4556 +size 160984 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_hip_rotation.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_hip_rotation.stl new file mode 100644 index 0000000..3ce3272 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_hip_rotation.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:42cc54c4458266d2ef0ef7669f3d3fe8812f710bcb4ba7c9d8b9f21e4c2040e7 +size 342784 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_shin.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_shin.stl new file mode 100644 index 0000000..8e371f6 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_shin.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:18f4e2117ee62abfc98ad1d26e75e40bbc521ccfd8e5a75802351f35a794233b +size 272484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_thigh.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_thigh.stl new file mode 100644 index 0000000..d15335c --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/assets/right_thigh.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:515fd1012f08458e213669e7bb96d421fe04ff4290662fe02700ed11c9c83d0c +size 362184 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/auto_gait.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/auto_gait.json new file mode 100644 index 0000000..2632f46 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/auto_gait.json @@ -0,0 +1,16 @@ +{ + "slow": 0.221, + "medium": 0.336, + "fast": 0.568, + "dx_max": [0, 0.1], + "dy_max": [0, 0.1], + "dtheta_max": [0, 0.25], + "min_sweep_x": -0.03, + "max_sweep_x": 0.08, + "min_sweep_y": -0.03, + "max_sweep_y": 0.03, + "min_sweep_theta": -0.1, + "max_sweep_theta": 0.1, + "sweep_xy_granularity": 0.02, + "sweep_theta_granularity": 0.03 +} diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/go_bdx.urdf b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/go_bdx.urdf new file mode 100644 index 0000000..23c1a93 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/go_bdx.urdf @@ -0,0 +1,459 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_defaults.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_defaults.json new file mode 100644 index 0000000..26e1702 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_defaults.json @@ -0,0 +1,53 @@ +{ + "dx": 0.0, + "dy": 0.0, + "dtheta": 0.0, + "duration": 5, + "hardware": true, + "trunk_mode": true, + "double_support_ratio": 0.2, + "startend_double_support_ratio": 1.5, + "planned_timesteps": 48, + "replan_timesteps": 10, + "walk_com_height": 0.26, + "walk_foot_height": 0.025, + "walk_trunk_pitch": -8, + "walk_foot_rise_ratio": 0.1, + "single_support_duration": 0.45, + "single_support_timesteps": 10, + "foot_length": 0.16, + "feet_spacing": 0.19, + "zmp_margin": 0.0, + "foot_zmp_target_x": 0.0, + "foot_zmp_target_y": 0.0, + "walk_max_dtheta": 1.0, + "walk_max_dy": 0.1, + "walk_max_dx_forward": 0.1, + "walk_max_dx_backward": 0.1, + "joints": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle" + ], + "joint_angles": { + "head_pitch": 0, + "head_yaw": 0, + "head_roll": 0, + "neck_pitch": 0, + "left_antenna": 0, + "right_antenna": 0 + } +} \ No newline at end of file diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_presets/.tmp/preset.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_presets/.tmp/preset.json new file mode 100644 index 0000000..63319d8 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_presets/.tmp/preset.json @@ -0,0 +1,53 @@ +{ + "dx": -0.2, + "dy": -0.2, + "dtheta": -0.2, + "duration": 5, + "hardware": true, + "trunk_mode": true, + "double_support_ratio": 0.2, + "startend_double_support_ratio": 1.5, + "planned_timesteps": 48, + "replan_timesteps": 10, + "walk_com_height": 0.26, + "walk_foot_height": 0.025, + "walk_trunk_pitch": 6, + "walk_foot_rise_ratio": 0.1, + "single_support_duration": 0.25, + "single_support_timesteps": 10, + "foot_length": 0.16, + "feet_spacing": 0.19, + "zmp_margin": 0.0, + "foot_zmp_target_x": 0.0, + "foot_zmp_target_y": 0.0, + "walk_max_dtheta": 1.0, + "walk_max_dy": 0.1, + "walk_max_dx_forward": 0.1, + "walk_max_dx_backward": 0.1, + "joints": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle" + ], + "joint_angles": { + "head_pitch": 0, + "head_yaw": 0, + "head_roll": 0, + "neck_pitch": 0, + "left_antenna": 0, + "right_antenna": 0 + } +} \ No newline at end of file diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_presets/fast.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_presets/fast.json new file mode 100644 index 0000000..5e8dc00 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_presets/fast.json @@ -0,0 +1,53 @@ +{ + "dx": 0.0, + "dy": 0.0, + "dtheta": 0.0, + "duration": 2, + "hardware": true, + "trunk_mode": true, + "double_support_ratio": 0.1, + "startend_double_support_ratio": 1.5, + "planned_timesteps": 48, + "replan_timesteps": 10, + "walk_com_height": 0.26, + "walk_foot_height": 0.025, + "walk_trunk_pitch": 6, + "walk_foot_rise_ratio": 0.1, + "single_support_duration": 0.2, + "single_support_timesteps": 10, + "foot_length": 0.16, + "feet_spacing": 0.19, + "zmp_margin": 0.0, + "foot_zmp_target_x": 0.0, + "foot_zmp_target_y": 0.0, + "walk_max_dtheta": 1.0, + "walk_max_dy": 0.1, + "walk_max_dx_forward": 0.1, + "walk_max_dx_backward": 0.1, + "joints": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle" + ], + "joint_angles": { + "head_pitch": 0, + "head_yaw": 0, + "head_roll": 0, + "neck_pitch": 0, + "left_antenna": 0, + "right_antenna": 0 + } +} diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_presets/medium.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_presets/medium.json new file mode 100644 index 0000000..9a08565 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_presets/medium.json @@ -0,0 +1,53 @@ +{ + "dx": 0.0, + "dy": 0.0, + "dtheta": 0.0, + "duration": 2, + "hardware": true, + "trunk_mode": true, + "double_support_ratio": 0.2, + "startend_double_support_ratio": 1.5, + "planned_timesteps": 48, + "replan_timesteps": 10, + "walk_com_height": 0.27, + "walk_foot_height": 0.025, + "walk_trunk_pitch": -2, + "walk_foot_rise_ratio": 0.1, + "single_support_duration": 0.30, + "single_support_timesteps": 10, + "foot_length": 0.16, + "feet_spacing": 0.19, + "zmp_margin": 0.0, + "foot_zmp_target_x": 0.0, + "foot_zmp_target_y": -0.04, + "walk_max_dtheta": 1.0, + "walk_max_dy": 0.1, + "walk_max_dx_forward": 0.1, + "walk_max_dx_backward": 0.1, + "joints": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle" + ], + "joint_angles": { + "head_pitch": 0, + "head_yaw": 0, + "head_roll": 0, + "neck_pitch": 0, + "left_antenna": 0, + "right_antenna": 0 + } +} \ No newline at end of file diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_presets/slow.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_presets/slow.json new file mode 100644 index 0000000..e5d6bf7 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/go_bdx/placo_presets/slow.json @@ -0,0 +1,53 @@ +{ + "dx": 0.0, + "dy": 0.0, + "dtheta": 0.0, + "duration": 2, + "hardware": true, + "trunk_mode": true, + "double_support_ratio": 0.3, + "startend_double_support_ratio": 1.5, + "planned_timesteps": 48, + "replan_timesteps": 10, + "walk_com_height": 0.27, + "walk_foot_height": 0.025, + "walk_trunk_pitch": -8, + "walk_foot_rise_ratio": 0.1, + "single_support_duration": 0.40, + "single_support_timesteps": 10, + "foot_length": 0.16, + "feet_spacing": 0.19, + "zmp_margin": 0.0, + "foot_zmp_target_x": 0.0, + "foot_zmp_target_y": -0.04, + "walk_max_dtheta": 1.0, + "walk_max_dy": 0.1, + "walk_max_dx_forward": 0.1, + "walk_max_dx_backward": 0.1, + "joints": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle" + ], + "joint_angles": { + "head_pitch": 0, + "head_yaw": 0, + "head_roll": 0, + "neck_pitch": 0, + "left_antenna": 0, + "right_antenna": 0 + } +} \ No newline at end of file diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/39281023.prt.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/39281023.prt.stl new file mode 100644 index 0000000..1bb7d08 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/39281023.prt.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aa8a9fa98ddf031e01f97e3d761149bdcd80017fa2196bc4bc7f92fcd7ab1c20 +size 20484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna.stl new file mode 100644 index 0000000..f283f0f --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2049b151008ccc0485468aae969e26d575ece2ddcdabaf3eb1624e5603ba9e24 +size 14484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna_holder_left.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna_holder_left.stl new file mode 100644 index 0000000..4c5cf51 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna_holder_left.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ff818140e5796d291956fdca5b3bb2ad3dd307bcb60f6f0acad6f20bcef205cf +size 102184 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna_holder_right.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna_holder_right.stl new file mode 100644 index 0000000..9e4d7c2 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna_holder_right.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9b7a19d00788d6f209fe998f94492bee426b501a5473d6cec7dfb953d27e1bfd +size 102184 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna_motor_holder.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna_motor_holder.stl new file mode 100644 index 0000000..c93309a --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna_motor_holder.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3c26e6b185786c881d8f81ee3401bbf2ed1fb462f09fff9f6ec5690409de2b1f +size 267684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna_tip.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna_tip.stl new file mode 100644 index 0000000..0157815 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/antenna_tip.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:badb981c8dbc8474f77eb700cda688fb0a3892555e2f97bbc7ab3cfbaaf713f8 +size 28884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/axis_to_axis.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/axis_to_axis.stl new file mode 100644 index 0000000..e80f6ba --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/axis_to_axis.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:638ed546070c58eb66d88c67925d141351514d300d92524b91f9a8c1e2970933 +size 223284 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/block_to_axis.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/block_to_axis.stl new file mode 100644 index 0000000..8e739e2 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/block_to_axis.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ca8218f432e72ec21a38c8c04c7e0a9bbcf11f4697fdf8d90c6a29c76f761ab8 +size 272084 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/bms.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/bms.stl new file mode 100644 index 0000000..1af5937 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/bms.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bab502befa20892f36900d118c4b72b0df1720abe4f5406dc53078d22fd5037a +size 684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/bno055.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/bno055.stl new file mode 100644 index 0000000..4517327 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/bno055.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ddac218abf25112abcd7267a8b6b957e0fcd2a3edbceb4b306b4ef58ca10e60b +size 73484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/board.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/board.stl new file mode 100644 index 0000000..0fbf779 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/board.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:03e95050cfbaa6e32bc4b5de7e49ce01502d70d27f3381678ddc5ec90a5f359a +size 368284 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/body_part.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/body_part.stl new file mode 100644 index 0000000..7833057 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/body_part.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a3c781448784f44674fd821f56b78314a266cd0d9b9b7239748cf7c37afed297 +size 359684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cage_back.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cage_back.stl new file mode 100644 index 0000000..5e34ebe --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cage_back.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:363eeb91e29a1ad237a768980d9f7bba9d7eb94ed05ef0bd2e9f248c4e8ef045 +size 222084 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cage_bottom.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cage_bottom.stl new file mode 100644 index 0000000..06d6f5b --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cage_bottom.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9c0646aa3c780035db9b2de2af2f775030dba019f3d612535a24ef930511ee3a +size 409584 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cage_bottom_battery_hold.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cage_bottom_battery_hold.stl new file mode 100644 index 0000000..5d9b95f --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cage_bottom_battery_hold.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:516dbf20073a06d0f31d2796c85c60333d7a988185074a047da8f4c311194165 +size 181984 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cage_top.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cage_top.stl new file mode 100644 index 0000000..7acf94d --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cage_top.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:022775f4a7dfe917e0e5dbd4fc66101cb53f93af8f3286fa20ea1b96bf7333b1 +size 139284 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cell.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cell.stl new file mode 100644 index 0000000..aa4eddc --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/cell.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:897b4ab0a3852aebd905009159345dacead2b1850a4f3a8e4c5d3095c108bc0f +size 273684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_case_b_dummy.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_case_b_dummy.stl new file mode 100644 index 0000000..edb3d62 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_case_b_dummy.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0d3d4ca97b51dced537229ce0526abee803291d5f698806979ade7e652d5a998 +size 248884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_case_f_dummy.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_case_f_dummy.stl new file mode 100644 index 0000000..3acffa0 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_case_f_dummy.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0548117d27199bc5ef9a613eab777ac34654e1bae45c47c444b5462396f57e78 +size 268984 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_case_m_dummy.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_case_m_dummy.stl new file mode 100644 index 0000000..d326bdc --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_case_m_dummy.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:80c922300ab7b7d435cf0430e7c23b8a33b399cca8766d852fdadcd595a55497 +size 162184 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_horn_dummy.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_horn_dummy.stl new file mode 100644 index 0000000..8d13f46 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_horn_dummy.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1d345b09b8d82695402d3dbb8dd69c3f7ff2edc84c6af8e9b173bf4ff4bcd332 +size 445184 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_horn_idle2_dummy.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_horn_idle2_dummy.stl new file mode 100644 index 0000000..1d4bbaf --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/dc15_a01_horn_idle2_dummy.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:916b5944f8d3e86f67b97a6987fdd306084485ab5605c538d890df5fed21eeff +size 296884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/double_u.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/double_u.stl new file mode 100644 index 0000000..d96894c --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/double_u.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:99b84c75188c33fbd5ff73bcd6619ac0a191c05532d80f70673316dc3da2bbb0 +size 295884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/foot.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/foot.stl new file mode 100644 index 0000000..50f7f1b --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/foot.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:49f444bfb19237597c86cea77efc6cb8c3656f2ac94542860eea322e4283cb37 +size 244784 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/foot_contact.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/foot_contact.stl new file mode 100644 index 0000000..2b73385 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/foot_contact.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:da81eefac08d09e4d59175cb9fa8bddbdc16cda68299bc4fdb8afdfd498de6e9 +size 78684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/front_cover.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/front_cover.stl new file mode 100644 index 0000000..265001c --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/front_cover.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1c52e336af2476490ddedb28df9495866e84369e76a3a99b3c98641c7db22352 +size 260384 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/head.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/head.stl new file mode 100644 index 0000000..e5f2d4f --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/head.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:75613c70c14bf1c90481457e32af8befc15031c3eecf523d8304fee366d1a147 +size 277084 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/head_roll_pitch.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/head_roll_pitch.stl new file mode 100644 index 0000000..87e252a --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/head_roll_pitch.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fc2fa025625777d902713bf8fc55ffb0485bd9d5266e25b12bc555ccc98a94d4 +size 250284 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/hip_left_roll_to_pitch.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/hip_left_roll_to_pitch.stl new file mode 100644 index 0000000..dfd2dde --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/hip_left_roll_to_pitch.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6841656f4d5bac02c8a961e947213336003677db337d8d1f2cb71134b38f67e6 +size 122384 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/hip_right_roll_to_pitch.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/hip_right_roll_to_pitch.stl new file mode 100644 index 0000000..d215992 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/hip_right_roll_to_pitch.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0aff8b038aea480cdbdcf584f4cd5b3540d7f3e6a6058150a861370b3146f19c +size 122784 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/holder.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/holder.stl new file mode 100644 index 0000000..4835cb6 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/holder.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a17ce27cfdfc489e18e7e2d1eaddb4b4d63612a48ae7fe8b20ffadadb618b076 +size 1484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/holder_u2d2_power_hub.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/holder_u2d2_power_hub.stl new file mode 100644 index 0000000..78b725c --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/holder_u2d2_power_hub.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f6d5191a5b9511202787439002814db91bac8a1a1a7417a8ce05b14e4dd87101 +size 155184 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b3b-eh-a.prt.1.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b3b-eh-a.prt.1.stl new file mode 100644 index 0000000..9d0671a --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b3b-eh-a.prt.1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:965661c7483aff33a74840b6133805440cfb132d7786c106e377bca4351c7aa5 +size 9884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b3b-eh-a.prt.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b3b-eh-a.prt.stl new file mode 100644 index 0000000..9d0671a --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b3b-eh-a.prt.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:965661c7483aff33a74840b6133805440cfb132d7786c106e377bca4351c7aa5 +size 9884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b4b-eh-a.prt.1.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b4b-eh-a.prt.1.stl new file mode 100644 index 0000000..e3dd85f --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b4b-eh-a.prt.1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ee7adaa9300e4ba604db4e96c4011f741132d7a28b1150bf6c5924ba653f3c5c +size 11484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b4b-eh-a.prt.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b4b-eh-a.prt.stl new file mode 100644 index 0000000..e3dd85f --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b4b-eh-a.prt.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ee7adaa9300e4ba604db4e96c4011f741132d7a28b1150bf6c5924ba653f3c5c +size 11484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b4b-ph-k-s.prt.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b4b-ph-k-s.prt.stl new file mode 100644 index 0000000..d5b012b --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/jst-b4b-ph-k-s.prt.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:da6aa57326a8a0556ecd922901cdc1aef8ba8c6a9fd13a42d156caa0c3e42296 +size 12284 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/micro_usb_2_0_connector__ab_rec.prt.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/micro_usb_2_0_connector__ab_rec.prt.stl new file mode 100644 index 0000000..b71a52b --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/micro_usb_2_0_connector__ab_rec.prt.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:70b6e59aef0d2df7e21c6205a986100e635618dbba56f1ff2715f8dd30ee76e1 +size 588284 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/pcb_u2d2.prt.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/pcb_u2d2.prt.stl new file mode 100644 index 0000000..2c3b551 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/pcb_u2d2.prt.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:edcc0337231b51f914fd97dea2dcf8f1d548b2493032e360ebd51d152f1338fe +size 38684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/power_switch.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/power_switch.stl new file mode 100644 index 0000000..9bc2bf0 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/power_switch.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6e45db557e01a37c4b48feae2136321420b50094ad7b42ed1943e6c591997133 +size 53384 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/rasp_spacer.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/rasp_spacer.stl new file mode 100644 index 0000000..1d38392 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/rasp_spacer.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7cc4e287e52fb45a190effb68f9e463fa69bc39f14349eef673ef13c2b3dca20 +size 28884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/raspberrypizerow.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/raspberrypizerow.stl new file mode 100644 index 0000000..3d86fa7 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/raspberrypizerow.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b4d89f6754730ac948d9dc6a31caf82251e54096cb87e78174e0fc1b2752ffca +size 1062884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/renfort_head_link.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/renfort_head_link.stl new file mode 100644 index 0000000..9b70eb7 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/renfort_head_link.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:053176b05ab92a37edbe21f58e09470641c9957ab75636b84cc1d5ca918e6e09 +size 29884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/renfort_leg.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/renfort_leg.stl new file mode 100644 index 0000000..f8ed759 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/renfort_leg.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:135c2efe6ed94f0f95ee5cfd6e54fdbbc768adf845e8347998f27c542047ea09 +size 29884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/sg90.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/sg90.stl new file mode 100644 index 0000000..28ff0a7 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/sg90.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b0f7d52b84744225c72b0ae4bb03210b9dbfd20c124cae02d471c30043725e80 +size 67284 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/spacer.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/spacer.stl new file mode 100644 index 0000000..c1f3018 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/spacer.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a2e5d8f7d4c9d5566ce0982dd49cfd9184387dae9da05da8869e908ffe7ba363 +size 59084 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/u2d2_casing.prt.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/u2d2_casing.prt.stl new file mode 100644 index 0000000..dbef116 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/u2d2_casing.prt.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ad350a4aa15b01fcf783c1ee4fd48e5b830797ad7bf101db78b96e1d8632cf86 +size 206484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/usb_c_charger.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/usb_c_charger.stl new file mode 100644 index 0000000..de62aa2 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/assets/usb_c_charger.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:66e15c92d7f9679c88de381724fad136fd0ee5b0b5d7810041219113b33afad3 +size 60684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/auto_gait.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/auto_gait.json new file mode 100644 index 0000000..54a2bf4 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/auto_gait.json @@ -0,0 +1,16 @@ +{ + "slow": 0.05, + "medium": 0.1, + "fast": 0.15, + "dx_max": [0, 0.05], + "dy_max": [0, 0.05], + "dtheta_max": [0, 0.25], + "min_sweep_x": -0.03, + "max_sweep_x": 0.08, + "min_sweep_y": -0.03, + "max_sweep_y": 0.03, + "min_sweep_theta": -0.1, + "max_sweep_theta": 0.1, + "sweep_xy_granularity": 0.02, + "sweep_theta_granularity": 0.03 +} diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/open_duck_mini.urdf b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/open_duck_mini.urdf new file mode 100644 index 0000000..49f1474 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/open_duck_mini.urdf @@ -0,0 +1,2573 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/placo_defaults.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/placo_defaults.json new file mode 100644 index 0000000..a0a837a --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/placo_defaults.json @@ -0,0 +1,50 @@ +{ + "dx": 0.04, + "dy": 0.0, + "dtheta": 0.0, + "duration": 8, + "hardware": true, + "double_support_ratio": 0.2, + "startend_double_support_ratio": 1.5, + "planned_timesteps": 48, + "replan_timesteps": 10, + "walk_com_height": 0.16, + "walk_foot_height": 0.03, + "walk_trunk_pitch": -5, + "walk_foot_rise_ratio": 0.2, + "single_support_duration": 0.18, + "single_support_timesteps": 10, + "foot_length": 0.06, + "feet_spacing": 0.14, + "zmp_margin": 0.0, + "foot_zmp_target_x": 0.0, + "foot_zmp_target_y": 0.0, + "walk_max_dtheta": 1.0, + "walk_max_dy": 0.1, + "walk_max_dx_forward": 0.08, + "walk_max_dx_backward": 0.03, + "joints": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle" + ], + "joint_angles": { + "head_pitch": -10, + "head_yaw": 0, + "neck_pitch": -10, + "left_antenna": 0, + "right_antenna": 0 + } +} diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/placo_presets/fast.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/placo_presets/fast.json new file mode 100644 index 0000000..6486e23 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/placo_presets/fast.json @@ -0,0 +1,50 @@ +{ + "dx": 0.0, + "dy": 0.0, + "dtheta": 0.0, + "duration": 5, + "hardware": true, + "double_support_ratio": 0.5, + "startend_double_support_ratio": 1.0, + "planned_timesteps": 48, + "replan_timesteps": 10, + "walk_com_height": 0.16, + "walk_foot_height": 0.02, + "walk_trunk_pitch": 7, + "walk_foot_rise_ratio": 0.05, + "single_support_duration": 0.15, + "single_support_timesteps": 10, + "foot_length": 0.06, + "feet_spacing": 0.14, + "zmp_margin": 0.0, + "foot_zmp_target_x": 0.0, + "foot_zmp_target_y": -0.04, + "walk_max_dtheta": 1.0, + "walk_max_dy": 0.1, + "walk_max_dx_forward": 0.08, + "walk_max_dx_backward": 0.03, + "joints": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle" + ], + "joint_angles": { + "head_pitch": 0, + "head_yaw": 0, + "neck_pitch": 0, + "left_antenna": 0, + "right_antenna": 0 + } +} diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/placo_presets/medium.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/placo_presets/medium.json new file mode 100644 index 0000000..af1304e --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini/placo_presets/medium.json @@ -0,0 +1,51 @@ +{ + "dx": 0.0, + "dy": 0.0, + "dtheta": 0.0, + "duration": 5, + "hardware": true, + "double_support_ratio": 0.6, + "startend_double_support_ratio": 1.0, + "planned_timesteps": 48, + "replan_timesteps": 10, + "walk_com_height": 0.16, + "walk_foot_height": 0.02, + "walk_trunk_pitch": -5, + "walk_foot_rise_ratio": 0.02, + "single_support_duration": 0.2, + "single_support_timesteps": 10, + "foot_length": 0.06, + "feet_spacing": 0.14, + "zmp_margin": 0.0, + "foot_zmp_target_x": 0.0, + "foot_zmp_target_y": -0.04, + "walk_max_dtheta": 1.0, + "walk_max_dy": 0.1, + "walk_max_dx_forward": 0.08, + "walk_max_dx_backward": 0.03, + "joints": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle" + ], + "joint_angles": { + "head_pitch": 0, + "head_yaw": 0, + "neck_pitch": 0, + "left_antenna": 0, + "right_antenna": 0 + } + +} diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/antenna.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/antenna.part new file mode 100644 index 0000000..ef03491 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/antenna.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ec07273bfd1edd5895d37e433ad661bff6073e04514965216655eb0577068abe +size 374 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/antenna.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/antenna.stl new file mode 100644 index 0000000..61d1916 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/antenna.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:40ac46ff9047d20cdf1154fba8125512f3ddad12e8ad4ebe0d6517b62754c4e0 +size 28884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/battery_pack_lid.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/battery_pack_lid.part new file mode 100644 index 0000000..02f13a1 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/battery_pack_lid.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:24c734aaaf611e25299674e6d1ac7fc6adc3b331353e2b3c4b538833cf1402bb +size 383 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/battery_pack_lid.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/battery_pack_lid.stl new file mode 100644 index 0000000..f690cce --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/battery_pack_lid.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d9a37ff5558c7fea666a43312483a0bf57eb1fa14003a0dd79459712f0dbe817 +size 119584 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/bms.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/bms.part new file mode 100644 index 0000000..66b2fa7 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/bms.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7f82c6cee611311f0953dde7dbdc4f4fb6ac23503e08a0529a43ae9defa2f0a2 +size 420 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/bms.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/bms.stl new file mode 100644 index 0000000..1af5937 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/bms.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bab502befa20892f36900d118c4b72b0df1720abe4f5406dc53078d22fd5037a +size 684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/bno055.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/bno055.part new file mode 100644 index 0000000..8419215 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/bno055.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c60fda5b5695d57e3f8d63d7b49ae25040a1e0486c401c05a528a9fbfe750fd2 +size 423 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/bno055.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/bno055.stl new file mode 100644 index 0000000..35e5b19 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/bno055.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:266027fa9c82ff00741b76ab6b97acf0788bf59574df671be7ee7e8926a5a268 +size 73484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/board.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/board.part new file mode 100644 index 0000000..fa2e9a4 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/board.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:39054a3fad8cea50d9aa309d60f5fe835089650d1e539dc60e40d9bf5a128685 +size 371 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/board.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/board.stl new file mode 100644 index 0000000..f1c7f88 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/board.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:697efdf8c7144ef4b56610e82ef334cb099f60a181d1b912f040e0afc91a9a77 +size 495884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_back.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_back.part new file mode 100644 index 0000000..175c9fe --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_back.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f3ef7345e869a8bbe0d60833cc49d1b21b3010207b0a4357ba008fc7bdf43ab4 +size 376 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_back.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_back.stl new file mode 100644 index 0000000..19da927 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_back.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:037383e5e5ba81a84cb6776edf253cfc6a5256b33133429e8efbd84387b486db +size 494784 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_front.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_front.part new file mode 100644 index 0000000..9ea7e47 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_front.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0409b1c07193b9ebd4b5afeebf4b7da7f75fbbc8f7db424a7ff72a3fcac282cd +size 377 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_front.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_front.stl new file mode 100644 index 0000000..d88827f --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_front.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b209dec3a53c0004a2ec173b227d4513f43eabd75887cfd5e916470a8d5bf6d4 +size 240184 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_middle_bottom.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_middle_bottom.part new file mode 100644 index 0000000..4163e0b --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_middle_bottom.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3b7981e5bbff35a4938846ec58de375ffec3616d6fc3c0748a9e3fdf59096041 +size 385 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_middle_bottom.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_middle_bottom.stl new file mode 100644 index 0000000..d5bfefe --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_middle_bottom.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:217315c97f20b5810cf662e7a38f91af10b5ab548a6fc38f6681c144c074db3b +size 418284 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_middle_top.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_middle_top.part new file mode 100644 index 0000000..2f5dd2c --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_middle_top.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8738bffe5d9a8f0959f7bebdb883ac69c9d97add0908aa82199cf8022bef95e6 +size 382 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_middle_top.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_middle_top.stl new file mode 100644 index 0000000..708b7aa --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/body_middle_top.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8145b0046d9e8cb624b3e1c7b1f2f9cf714ff9aa3b5b896f230b1460ca0e76db +size 300684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/cell.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/cell.part new file mode 100644 index 0000000..6fdaeaf --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/cell.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:25367a73bd02720caebb111c02ef6e02438dec1a0daca7796120e5927d3ff898 +size 421 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/cell.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/cell.stl new file mode 100644 index 0000000..aa4eddc --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/cell.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:897b4ab0a3852aebd905009159345dacead2b1850a4f3a8e4c5d3095c108bc0f +size 273684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/drive_palonier.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/drive_palonier.part new file mode 100644 index 0000000..d85da0f --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/drive_palonier.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:900ab7ebc7a30c6f8374f16c7498cb6d1e0e8225cbc16e7eeba579a9b151d6ee +size 380 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/drive_palonier.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/drive_palonier.stl new file mode 100644 index 0000000..2640196 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/drive_palonier.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:70ba3a79ca51aa6c39739cba934d0cf91ad3a6115da3d995cadff1811080c217 +size 102584 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_bottom_pla.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_bottom_pla.part new file mode 100644 index 0000000..542a1d8 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_bottom_pla.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bfa4d8226698dd8bf509230044f1bd94c8e75a2221eb60c7c38abe810e39b944 +size 382 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_bottom_pla.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_bottom_pla.stl new file mode 100644 index 0000000..af292e1 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_bottom_pla.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:da7190dbcdcc013cece385afad9558436983d4ecd73bc62b1ec7484092795da3 +size 59284 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_bottom_tpu.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_bottom_tpu.part new file mode 100644 index 0000000..baf922e --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_bottom_tpu.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:51de0cdf045f190baacb15e3dd7e89d1d50fffce753a17bc5037037721c918f1 +size 382 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_bottom_tpu.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_bottom_tpu.stl new file mode 100644 index 0000000..18cf8a7 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_bottom_tpu.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3d0329511c6a4d8796f9ad425dfa3019c6e2d7a7c50ae057e44eaaee636fc31a +size 102784 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_side.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_side.part new file mode 100644 index 0000000..6a76068 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_side.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:37d0be00926f69fa7c1af3a7a2e06a6c908ec6ed9f9a38d3082c81568cea5994 +size 376 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_side.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_side.stl new file mode 100644 index 0000000..6e7e7dd --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_side.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8724b025390917bb3a251b8541f934b5a3160c11257895a0b76bbdf486681ad9 +size 244784 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_top.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_top.part new file mode 100644 index 0000000..6b9c7ed --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_top.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:32f47e16e33f19667a7eca71b21c403dbc91e70f88a65679854a17e8610dada2 +size 375 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_top.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_top.stl new file mode 100644 index 0000000..18900f6 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/foot_top.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f55a1a1b2376e248cf3db91e5bc9e7b525d63827d3b9b3da46205f3c33976752 +size 246684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head.part new file mode 100644 index 0000000..5c8dbda --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2a9d8c740ab2cd212d376f5e927e3c8b9f0ced2e78edb072133dacf8f563d20f +size 371 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head.stl new file mode 100644 index 0000000..8d09f92 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4c77dee05749f9a9785be586a3fae013079bd4bc4ff58fd78334514e860d61aa +size 1081784 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_bot_sheet.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_bot_sheet.part new file mode 100644 index 0000000..6462fb3 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_bot_sheet.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d9b2d76d8ed017dc1b19d45088c308577d39c0f0efad44e6bc16d537c8236d14 +size 381 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_bot_sheet.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_bot_sheet.stl new file mode 100644 index 0000000..f8d8dd0 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_bot_sheet.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:56f62e32612154c33f42d0c1550fc5b4348cd65bb0df03d7c3552ba3a6400f90 +size 292084 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_pitch_to_yaw.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_pitch_to_yaw.part new file mode 100644 index 0000000..e3355e0 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_pitch_to_yaw.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:323bdc637638dad69a65b5e0b9d0a6a878f1697863f2bc846ef6be0347628f5c +size 384 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_pitch_to_yaw.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_pitch_to_yaw.stl new file mode 100644 index 0000000..ba1b6b1 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_pitch_to_yaw.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8d6cb940bdcf84d40160939a4bdd29a27a8da1650eec6aa3e2d1dd5eefa07f39 +size 673084 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_roll_mount.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_roll_mount.part new file mode 100644 index 0000000..c02d19f --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_roll_mount.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a1582d102ac57d1228f93dcaf7dda68572b207d3f87e6df1f234361c15facf56 +size 382 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_roll_mount.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_roll_mount.stl new file mode 100644 index 0000000..8b411b6 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_roll_mount.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:edd133c532498b48b505c02231c140a8ea38f64b38db48b8d17977fbee97e1c4 +size 281484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_yaw_to_roll.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_yaw_to_roll.part new file mode 100644 index 0000000..5519b11 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_yaw_to_roll.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2673ff34e542290b586cdc36979b994d41c513f8f66d03a1260345c05aec28cf +size 383 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_yaw_to_roll.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_yaw_to_roll.stl new file mode 100644 index 0000000..83aaf40 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/head_yaw_to_roll.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fb3e4d4b2b9e575a8364ddb0f82b2909c1cfe5c7e3321e5c3e9683d3ae261482 +size 348284 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/holder.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/holder.part new file mode 100644 index 0000000..56501b1 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/holder.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f85e58191b0c5f18dd8e605ced2700c5b5f38cc54bc21868200bd7d08d18ded4 +size 423 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/holder.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/holder.stl new file mode 100644 index 0000000..4835cb6 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/holder.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a17ce27cfdfc489e18e7e2d1eaddb4b4d63612a48ae7fe8b20ffadadb618b076 +size 1484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_antenna_holder.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_antenna_holder.part new file mode 100644 index 0000000..17e7fbe --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_antenna_holder.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f3751a50443115aef23e4bf2ab842b5ae2286e383102fa291276aa4c757d0982 +size 386 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_antenna_holder.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_antenna_holder.stl new file mode 100644 index 0000000..88f6b2c --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_antenna_holder.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0fae00f3ea6defb945f3541805dcdca2277d98eb891d6c20a46d0af2a215ba33 +size 108484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_cache.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_cache.part new file mode 100644 index 0000000..9c81b79 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_cache.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c9ef32d2f04531094844a3543f539396465ddf2b835c0464a7c785fc87204f78 +size 377 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_cache.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_cache.stl new file mode 100644 index 0000000..738dd50 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_cache.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8ba1ec39e643b0ba932eb7de9e6327c8bf7d2991cd8642068a99f8038963faf4 +size 107284 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_knee_to_ankle_left_sheet.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_knee_to_ankle_left_sheet.part new file mode 100644 index 0000000..a83ce7c --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_knee_to_ankle_left_sheet.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d7a89defe831b7bbaec04c993c7bd195ee394036e931c3ac710f98747f40ebbd +size 396 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_knee_to_ankle_left_sheet.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_knee_to_ankle_left_sheet.stl new file mode 100644 index 0000000..5b0a952 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_knee_to_ankle_left_sheet.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c49d18abcd227c01132ffbf1825c4c2e8f46b27f90cb08936d4a955b09532dec +size 284384 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_knee_to_ankle_right_sheet.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_knee_to_ankle_right_sheet.part new file mode 100644 index 0000000..d7e62ae --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_knee_to_ankle_right_sheet.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:065342e11e58cbd631f8b30290685001498a516d3a82dadfc325c65ab06fc40d +size 397 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_knee_to_ankle_right_sheet.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_knee_to_ankle_right_sheet.stl new file mode 100644 index 0000000..a904543 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_knee_to_ankle_right_sheet.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:831703441094337352fec9adef68c79b660c25e643e68b19aefb2f088b6fb673 +size 289984 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_roll_to_pitch.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_roll_to_pitch.part new file mode 100644 index 0000000..d40d484 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_roll_to_pitch.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:172d3243d065326787608ebca38ddffccbe5a5b09a81cecf2c2231110c4110ea +size 385 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_roll_to_pitch.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_roll_to_pitch.stl new file mode 100644 index 0000000..b0d78a2 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/left_roll_to_pitch.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6f876a11c3c2076ddc9d437948aba94217dad5de75dba9e1f5fab663e8598379 +size 504784 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/leg_spacer.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/leg_spacer.part new file mode 100644 index 0000000..1670f0a --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/leg_spacer.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9baa23fd0f9205a5b1539da0b1606c88c08f84a9e820675c9894df4baf8b6c18 +size 377 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/leg_spacer.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/leg_spacer.stl new file mode 100644 index 0000000..d61a42a --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/leg_spacer.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f7e2d0c4ca470032ce6c5cce07447a4c321b6f217a14d1e467cbee972344c14d +size 159084 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/neck_left_sheet.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/neck_left_sheet.part new file mode 100644 index 0000000..e17d375 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/neck_left_sheet.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d0c29a198b4171291be0c920030e7ff8e0c4e645d272bed588a1063075098876 +size 382 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/neck_left_sheet.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/neck_left_sheet.stl new file mode 100644 index 0000000..f1e9d88 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/neck_left_sheet.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aa3202e5304cd7c11ee3b9c9a39c3d4c2646aeb3e79e8901e731134afb8514f9 +size 211484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/neck_right_sheet.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/neck_right_sheet.part new file mode 100644 index 0000000..d660865 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/neck_right_sheet.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b9b3141d0cab07306666b4fb8e4bfc2ef56bb0ae90fc9352f03837d7543f85da +size 383 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/neck_right_sheet.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/neck_right_sheet.stl new file mode 100644 index 0000000..bdc6d8c --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/neck_right_sheet.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e41167546cc7fe9f5a5104c020a885453a960019692b02499f7148c0a0504ec5 +size 215684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/passive_palonier.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/passive_palonier.part new file mode 100644 index 0000000..2da43f0 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/passive_palonier.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3576130569cd7d9d0dbe8fef5a63c18dcf7bbbf24def2943da5e32df7def725e +size 382 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/passive_palonier.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/passive_palonier.stl new file mode 100644 index 0000000..fa67e69 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/passive_palonier.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:88f1ac81c6214080d7ef083a5faafcde9e986b551136e531a1ac7bb1b6b11a4a +size 102484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/power_switch.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/power_switch.part new file mode 100644 index 0000000..143fc6f --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/power_switch.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cb759c5c4343783355a62db8133eff78844199ac40cc7b37f4ef6e9108d1bbdb +size 429 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/power_switch.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/power_switch.stl new file mode 100644 index 0000000..a8df5a5 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/power_switch.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3a6ff93634f42a231845ead79c6fe3739f80b2859e11ca92d95110dad00a2dc2 +size 96584 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/raspberrypizerow.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/raspberrypizerow.part new file mode 100644 index 0000000..ac6a602 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/raspberrypizerow.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:94f45c3a5ebd11adc867b3cb487f47759a552e1559de7fb647003abeb082a6f0 +size 433 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/raspberrypizerow.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/raspberrypizerow.stl new file mode 100644 index 0000000..8d799a8 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/raspberrypizerow.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a153a352da3a7bd748fa71c0451318c2c2a9e5fb9d5f4e6f606a8329505e10e2 +size 1062884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_antenna_holder.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_antenna_holder.part new file mode 100644 index 0000000..b3c548b --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_antenna_holder.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:22bb80ce4ec1d2be784573ff9db9b18582362e9b1a71c7c4e07ed1cb176c454d +size 387 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_antenna_holder.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_antenna_holder.stl new file mode 100644 index 0000000..a8d9ea5 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_antenna_holder.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bbf0d1beb0625fccdcafedb9bb6a46592c852ca0904636e78f040613ab4f7419 +size 108484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_cache.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_cache.part new file mode 100644 index 0000000..e34996e --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_cache.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5fea67f33caee5b13984ab150095f037203558b567948ba0a740f26a87f1c980 +size 378 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_cache.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_cache.stl new file mode 100644 index 0000000..d48742b --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_cache.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7cf3edfd76be032fcb657ded24e7b098a7c4107919f6420073ae86276df9d002 +size 107284 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_roll_to_pitch.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_roll_to_pitch.part new file mode 100644 index 0000000..15c4713 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_roll_to_pitch.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:413be0c3e49ee21061d98556a16e5c4948f56fcb8fbc39a86e0b105fb61513fe +size 386 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_roll_to_pitch.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_roll_to_pitch.stl new file mode 100644 index 0000000..e0fbcfc --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/right_roll_to_pitch.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:60c5ac394fceafa042103745cca33d09dc879ba78867c76cee5950c3f2d59b43 +size 504684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_bearing.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_bearing.part new file mode 100644 index 0000000..e377a9d --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_bearing.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ff65dd49e97547fc54a564fc4c253d20864153495afb1d9ae1e34e8137597d6f +size 379 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_bearing.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_bearing.stl new file mode 100644 index 0000000..f69e94c --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_bearing.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:777abd3d00a58c06bf31f8cc233fc8620099ec7e8d79eb7325dc8c1cdd7aa297 +size 28884 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_motor_bottom.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_motor_bottom.part new file mode 100644 index 0000000..100fde4 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_motor_bottom.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3121817ed2d21290a61368c056f6e10f39ec62f5b68eec3e146a93f8b4b097da +size 384 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_motor_bottom.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_motor_bottom.stl new file mode 100644 index 0000000..06a7069 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_motor_bottom.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:20c85b387cd88a8939e70d86cdfc1556f562c8c3e39fc105cfe84e50c977e60a +size 89584 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_motor_top.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_motor_top.part new file mode 100644 index 0000000..53a6034 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_motor_top.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5034209cc4c58b36fc7307e99d89176e9175e6b8dc8d76238a8a9a59270ea58d +size 381 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_motor_top.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_motor_top.stl new file mode 100644 index 0000000..44fe8fb --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/roll_motor_top.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e9b6e70d23ad1aee3704de2b2240d3450daaa69d55429a57e50702d98c8c8fb4 +size 226584 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/sg90.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/sg90.part new file mode 100644 index 0000000..dd7f112 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/sg90.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f70d2cf44b380be6f3eb769ecbbffad4d0d8ba67f15fd39e34231942822e9899 +size 421 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/sg90.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/sg90.stl new file mode 100644 index 0000000..54ff075 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/sg90.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5594cd7f8041e407ea3e7260efa9804055d8c08e3d35ec2282a04169387c3d7f +size 67284 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/trunk_bottom.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/trunk_bottom.part new file mode 100644 index 0000000..1d11b21 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/trunk_bottom.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c53eba1cebc14a9da78fa7cc871e208f0876a1c3f58eef493778ec1d3e539e78 +size 379 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/trunk_bottom.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/trunk_bottom.stl new file mode 100644 index 0000000..58ac00f --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/trunk_bottom.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b6f5baba76f9f29d2f76714630927f20a5358c3096b682b1129866cc164c7772 +size 282484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/trunk_top.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/trunk_top.part new file mode 100644 index 0000000..cafe768 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/trunk_top.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2af987d09d0abde748e28a2e71ec810701539d1ec9053716f80def53c4ec68e7 +size 376 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/trunk_top.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/trunk_top.stl new file mode 100644 index 0000000..6a6f90a --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/trunk_top.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:693314821a26e943df0c4500b8e35fbe84e1fa8d8348fdc5fa5adfc0c2f98c7c +size 951484 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/usb_c_charger.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/usb_c_charger.part new file mode 100644 index 0000000..4896632 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/usb_c_charger.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5c1e68cab6908b2168459f686f9589be5ad953e0038cecf59ba6deef7ba54956 +size 430 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/usb_c_charger.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/usb_c_charger.stl new file mode 100644 index 0000000..c47685e --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/usb_c_charger.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:485a27a44d353343f18de679c5ce2cfd87cc662150d379d37b79d6869f358f8a +size 60684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0122topcabinetcase_95.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0122topcabinetcase_95.part new file mode 100644 index 0000000..f2084e7 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0122topcabinetcase_95.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6d890643ddbd206598492117912af352bcd85da22499a8db20dd8798fa0252ff +size 395 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0122topcabinetcase_95.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0122topcabinetcase_95.stl new file mode 100644 index 0000000..0268c81 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0122topcabinetcase_95.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aa68a42a6e95bf1c90f2875232804982630ec1d6011efba559a7b7681c595cb4 +size 2068984 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0123middlecase_56.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0123middlecase_56.part new file mode 100644 index 0000000..b34e1e6 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0123middlecase_56.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:dbe8ef46ef68d95e7e817048b0cbf2d9ca363936e2fa6e7f5ce636efe61e4320 +size 391 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0123middlecase_56.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0123middlecase_56.stl new file mode 100644 index 0000000..3dca5b9 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0123middlecase_56.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2d0c841a7d7f2bb7cdb17ee7cb7cab7871cc6bb33ca3cee375beab2fdac2f6b4 +size 49684 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0124bottomcase_45.part b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0124bottomcase_45.part new file mode 100644 index 0000000..fe32cca --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0124bottomcase_45.part @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d61b1123c337ac59e4731793fb301ae74ad0e95bc6a707217fde3cd596ae5f92 +size 391 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0124bottomcase_45.stl b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0124bottomcase_45.stl new file mode 100644 index 0000000..6ddbe0e --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/assets/wj-wk00-0124bottomcase_45.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bf0d8a2b7c00710e6c5e6a0f100f3dc27132e6d0bac215480d37dc3236395349 +size 5117184 diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/auto_gait.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/auto_gait.json new file mode 100644 index 0000000..36c1c45 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/auto_gait.json @@ -0,0 +1,16 @@ +{ + "slow": 0.05, + "medium": 0.1, + "fast": 0.15, + "dx_max": [0, 0.05], + "dy_max": [0, 0.05], + "dtheta_max": [0, 0.25], + "min_sweep_x": -0.04, + "max_sweep_x": 0.06, + "min_sweep_y": -0.03, + "max_sweep_y": 0.03, + "min_sweep_theta": -0.3, + "max_sweep_theta": 0.3, + "sweep_xy_granularity": 0.02, + "sweep_theta_granularity": 0.07 +} diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/open_duck_mini_v2.urdf b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/open_duck_mini_v2.urdf new file mode 100644 index 0000000..8ca5901 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/open_duck_mini_v2.urdf @@ -0,0 +1,2554 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/placo_defaults.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/placo_defaults.json new file mode 100644 index 0000000..3378cab --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/placo_defaults.json @@ -0,0 +1,52 @@ +{ + "dx": 0.1, + "dy": 0.0, + "dtheta": 0.0, + "duration": 8, + "hardware": true, + "double_support_ratio": 0.18, + "startend_double_support_ratio": 1.5, + "planned_timesteps": 48, + "replan_timesteps": 10, + "walk_com_height": 0.2, + "walk_foot_height": 0.04, + "walk_trunk_pitch": -3, + "walk_foot_rise_ratio": 0.2, + "single_support_duration": 0.17, + "single_support_timesteps": 10, + "foot_length": 0.06, + "feet_spacing": 0.16, + "zmp_margin": 0.0, + "foot_zmp_target_x": 0.0, + "foot_zmp_target_y": 0.0, + "walk_max_dtheta": 1.0, + "walk_max_dy": 0.1, + "walk_max_dx_forward": 0.08, + "walk_max_dx_backward": 0.03, + "joints": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle" + ], + "joint_angles": { + "head_pitch": 0, + "head_yaw": 0, + "neck_pitch": 0, + "head_roll": 0, + "left_antenna": 0, + "right_antenna": 0 + } +} diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/placo_presets/fast.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/placo_presets/fast.json new file mode 100644 index 0000000..6a985b6 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/placo_presets/fast.json @@ -0,0 +1,52 @@ +{ + "dx": 0.0, + "dy": 0.0, + "dtheta": 0.0, + "duration": 8, + "hardware": true, + "double_support_ratio": 0.5, + "startend_double_support_ratio": 1.0, + "planned_timesteps": 48, + "replan_timesteps": 10, + "walk_com_height": 0.21, + "walk_foot_height": 0.04, + "walk_trunk_pitch": 5, + "walk_foot_rise_ratio": 0.02, + "single_support_duration": 0.18, + "single_support_timesteps": 10, + "foot_length": 0.06, + "feet_spacing": 0.16, + "zmp_margin": 0.0, + "foot_zmp_target_x": 0.0, + "foot_zmp_target_y": -0.03, + "walk_max_dtheta": 1.0, + "walk_max_dy": 0.1, + "walk_max_dx_forward": 0.08, + "walk_max_dx_backward": 0.03, + "joints": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle" + ], + "joint_angles": { + "neck_pitch": 0, + "head_pitch": 0, + "head_yaw": 0, + "head_roll": 0, + "left_antenna": 0, + "right_antenna": 0 + } +} diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/placo_presets/medium.json b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/placo_presets/medium.json new file mode 100644 index 0000000..c734273 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/robots/open_duck_mini_v2/placo_presets/medium.json @@ -0,0 +1,52 @@ +{ + "dx": 0.0, + "dy": 0.0, + "dtheta": 0.0, + "duration": 8, + "hardware": true, + "double_support_ratio": 0.5, + "startend_double_support_ratio": 1.0, + "planned_timesteps": 48, + "replan_timesteps": 10, + "walk_com_height": 0.205, + "walk_foot_height": 0.04, + "walk_trunk_pitch": -4, + "walk_foot_rise_ratio": 0.02, + "single_support_duration": 0.18, + "single_support_timesteps": 10, + "foot_length": 0.06, + "feet_spacing": 0.16, + "zmp_margin": 0.0, + "foot_zmp_target_x": 0.0, + "foot_zmp_target_y": -0.03, + "walk_max_dtheta": 1.0, + "walk_max_dy": 0.1, + "walk_max_dx_forward": 0.08, + "walk_max_dx_backward": 0.03, + "joints": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle" + ], + "joint_angles": { + "neck_pitch": 20, + "head_pitch": -26, + "head_yaw": 0, + "head_roll": 0, + "left_antenna": 0, + "right_antenna": 0 + } +} \ No newline at end of file diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/templates/index.html b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/templates/index.html new file mode 100644 index 0000000..49d6988 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/open_duck_reference_motion_generator/templates/index.html @@ -0,0 +1,489 @@ + + + + + + Placo Gait Generator + + + +

Placo Gait Generator

+ + +
+ + +
+ +
+ + +
+ +
+ +
+ +
+ + {{ parameters.dx }} +
+
+ +
+ +
+ + {{ parameters.dy }} +
+
+ +
+ +
+ +
+ +
+ + {{ parameters.dtheta }} +
+
+ +
+ +
+ + {{ parameters.duration }} +
+
+
+
+ + + +
+ +
+ +
+ +
+ + {{ parameters.double_support_ratio }} +
+
+ +
+ +
+ + {{ parameters.startend_double_support_ratio }} +
+
+ +
+ +
+ + {{ parameters.planned_timesteps }} +
+
+ +
+ +
+ + {{ parameters.replan_timesteps }} +
+
+ +
+ +
+ + {{ parameters.walk_com_height }} +
+
+ +
+ +
+ + {{ parameters.walk_foot_height }} +
+
+ +
+ +
+ + {{ parameters.walk_trunk_pitch }} +
+
+ +
+ +
+ + {{ parameters.walk_foot_rise_ratio }} +
+
+ +
+ +
+ + {{ parameters.single_support_duration }} +
+
+ +
+ +
+ + {{ parameters.single_support_timesteps }} +
+
+
+ + +
+ +
+ +
+ + {{ parameters.foot_length }} +
+
+ +
+ +
+ + {{ parameters.feet_spacing }} +
+
+ +
+ +
+ + {{ parameters.zmp_margin }} +
+
+ +
+ +
+ + {{ parameters.foot_zmp_target_x }} +
+
+ +
+ +
+ + {{ parameters.foot_zmp_target_y }} +
+
+ +
+ +
+ + {{ parameters.walk_max_dtheta }} +
+
+ +
+ +
+ + {{ parameters.walk_max_dy }} +
+
+ +
+ +
+ + {{ parameters.walk_max_dx_forward }} +
+
+ +
+ +
+ + {{ parameters.walk_max_dx_backward }} +
+
+
+
+
+ +
+ + + + + + + + + +
+ + + + diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/pyproject.toml b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/pyproject.toml new file mode 100644 index 0000000..817c3f5 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/pyproject.toml @@ -0,0 +1,15 @@ +[project] +name = "open_duck_reference_motion_generator" +version = "0.1.0" +description = "Add your description here" +readme = "README.md" +requires-python = "==3.10.12" +dependencies = [ + "flask>=3.1.0", + "framesviewer>=1.0.2", + "matplotlib>=3.10.0", + "placo==0.6.3", + "pygame>=2.6.1", + "scikit-learn>=1.6.1", + "scipy>=1.15.2", +] diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/auto_waddle.py b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/auto_waddle.py new file mode 100644 index 0000000..3078dac --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/auto_waddle.py @@ -0,0 +1,354 @@ +import os +import re +import json +import time +import argparse +import subprocess +from concurrent.futures import ThreadPoolExecutor +import numpy as np + +SCRIPT_PATH = os.path.dirname(os.path.abspath(__file__)) + + +def run_command_with_logging(cmd_log_tuple): + cmd, log_file = cmd_log_tuple + if log_file is None: + print(f"cmd: {cmd}") + subprocess.run(cmd) + else: + with open(log_file, "w") as outfile: + subprocess.run(cmd, stdout=outfile, stderr=subprocess.STDOUT) + + +def numeric_prefix_sort_key(item): + total_speed, preset_name = item + match = re.match(r"(\d+)(.*)", preset_name) + if match: + number_part = int(match.group(1)) + rest_part = match.group(2) + return (number_part, rest_part) + return (float("inf"), preset_name) + + +def main(args): + """ + Generates random preset data, creates gait motions, filters recordings, + and (optionally) plots anim/sim if --plot is given. + """ + + start_time = time.time() + + # --------------------------------------------------------------- + # 1. Load parameters from auto_gait.json + # --------------------------------------------------------------- + # This JSON should contain keys like: slow, medium, fast, dx_max, dy_max, dtheta_max + props_path = f"{SCRIPT_PATH}/../open_duck_reference_motion_generator/robots/{args.duck}/auto_gait.json" + if not os.path.isfile(props_path): + raise FileNotFoundError(f"Could not find props file at: {props_path}") + + with open(props_path, "r") as f: + gait_props = json.load(f) + + # Extract needed values + slow = gait_props["slow"] + medium = gait_props["medium"] + fast = gait_props["fast"] + dx_max = gait_props["dx_max"] # e.g. [0, 0.05] + dy_max = gait_props["dy_max"] # e.g. [0, 0.05] + dtheta_max = gait_props["dtheta_max"] # e.g. [0, 0.25] + min_sweep_x = gait_props["min_sweep_x"] + max_sweep_x = gait_props["max_sweep_x"] + min_sweep_y = gait_props["min_sweep_y"] + max_sweep_y = gait_props["max_sweep_y"] + min_sweep_theta = gait_props["min_sweep_theta"] + max_sweep_theta = gait_props["max_sweep_theta"] + sweep_xy_granularity = gait_props["sweep_xy_granularity"] + sweep_theta_granularity = gait_props["sweep_theta_granularity"] + + # For convenience, keep these in a dict for the gait motions + gait_speeds = {"slow": slow, "medium": medium, "fast": fast} + + # Define the typical gait motions we want to generate + gait_motions = [ + "standing", + "forward", + "backward", + "left", + "right", + "ang_left", + "ang_right", + "dia_forward", + "dia_backward", + ] + + # --------------------------------------------------------------- + # 2. Paths and directories + # --------------------------------------------------------------- + presets_dir = f"{SCRIPT_PATH}/../open_duck_reference_motion_generator/robots/{args.duck}/placo_presets" + tmp_dir = os.path.join(presets_dir, "tmp") + os.makedirs(tmp_dir, exist_ok=True) + + log_dir = os.path.join(args.output_dir, "log") + os.makedirs(log_dir, exist_ok=True) + + # --------------------------------------------------------------- + # 3. Generate random presets (n times) + # "medium" and "fast" as example base speeds + # --------------------------------------------------------------- + preset_speeds = ["medium"] # , "fast"] + + if args.sweep: + dxs = np.arange(min_sweep_x, max_sweep_x + sweep_xy_granularity, sweep_xy_granularity) + dys = np.arange(min_sweep_y, max_sweep_y + sweep_xy_granularity, sweep_xy_granularity) + dthetas = np.arange(min_sweep_theta, max_sweep_theta + sweep_theta_granularity, sweep_theta_granularity) + all_n = len(dxs) * len(dys) * len(dthetas) + else: + all_n = args.num + + nb_moves_message = f"=== GENERATING {all_n} MOVES ===" + spacer = "=" * len(nb_moves_message) + print(spacer) + print(nb_moves_message) + print(spacer) + + commands = [] + for i in range(all_n): + # Randomly select one of those base speeds + selected_speed = np.random.choice(preset_speeds) + # Load the corresponding .json preset from placo_presets + preset_file = os.path.join(presets_dir, f"{selected_speed}.json") + if not os.path.isfile(preset_file): + raise FileNotFoundError(f"Preset file not found: {preset_file}") + + with open(preset_file, "r") as file: + data = json.load(file) + + if args.sweep: + dx_idx = i % len(dxs) + dy_idx = (i // len(dxs)) % len(dys) + dtheta_idx = (i // (len(dxs) * len(dys))) % len(dthetas) + + data["dx"] = round(dxs[dx_idx], 2) + data["dy"] = round(dys[dy_idx], 2) + data["dtheta"] = round(dthetas[dtheta_idx], 2) + else: + # Randomize dx, dy, dtheta within the specified max ranges + data["dx"] = round( + np.random.uniform(dx_max[0], dx_max[1]) * np.random.choice([-1, 1]), 2 + ) + data["dy"] = round( + np.random.uniform(dy_max[0], dy_max[1]) * np.random.choice([-1, 1]), 2 + ) + data["dtheta"] = round( + np.random.uniform(dtheta_max[0], dtheta_max[1]) + * np.random.choice([-1, 1]), + 2, + ) + + # Write to tmp_preset + tmp_preset = os.path.join(tmp_dir, f"{i}_{selected_speed}.json") + with open(tmp_preset, "w") as out_file: + json.dump(data, out_file, indent=4) + + # Call gait_generator (no bdx-specific arguments, references removed) + cmd = [ + "python", + f"{SCRIPT_PATH}/../open_duck_reference_motion_generator/gait_generator.py", + "--duck", + args.duck, + "--preset", + tmp_preset, + "--name", + str(i), + "--output_dir", + args.output_dir, + ] + log_file = None if args.verbose else os.path.join(log_dir, f"{i}.log") + commands.append((cmd, log_file)) + + # # --------------------------------------------------------------- + # # 4. Generate gait motions for slow/medium/fast speeds + # # --------------------------------------------------------------- + # for gait_speed_key, gait_speed_val in gait_speeds.items(): + # for gait_motion in gait_motions: + # # Base preset is e.g. "slow.json", "medium.json", or "fast.json" + # base_preset_path = os.path.join(presets_dir, f"{gait_speed_key}.json") + # if not os.path.isfile(base_preset_path): + # print(f"Skipping: no preset file {base_preset_path}") + # continue + + # with open(base_preset_path, "r") as file: + # data = json.load(file) + + # # Modify dx/dy/dtheta depending on the motion + # if gait_speed_key == "slow" and gait_motion == "standing": + # # Do nothing for slow standing + # pass + # elif gait_motion == "forward": + # data["dx"] = gait_speed_val + # elif gait_motion == "backward": + # data["dx"] = -gait_speed_val + # elif gait_motion == "left": + # data["dy"] = gait_speed_val + # elif gait_motion == "right": + # data["dy"] = -gait_speed_val + # elif gait_motion == "dia_forward": + # data["dx"] = gait_speed_val / 2 + # data["dy"] = gait_speed_val / 2 + # elif gait_motion == "dia_backward": + # data["dx"] = -gait_speed_val / 2 + # data["dy"] = -gait_speed_val / 2 + # elif gait_motion == "ang_left": + # data["dtheta"] = gait_speed_val + # elif gait_motion == "ang_right": + # data["dtheta"] = -gait_speed_val + + # # Save to a temporary preset + # motion_name = f"{gait_motion}_{gait_speed_key}" + # motion_preset_path = os.path.join( + # tmp_dir, f"motion_preset_{motion_name}.json" + # ) + # with open(motion_preset_path, "w") as out_file: + # json.dump(data, out_file, indent=4) + + # # Run gait_generator + # cmd = [ + # "python", + # f"{SCRIPT_PATH}/../open_duck_reference_motion_generator/gait_generator.py", + # "--duck", + # args.duck, + # "--preset", + # motion_preset_path, + # "--name", + # motion_name, + # "--output_dir", + # args.output_dir + # ] + # log_file = ( + # None if args.verbose else os.path.join(log_dir, f"{motion_name}.log") + # ) + # commands.append((cmd, log_file)) + + # for command in commands: + + # print(command) + # exit() + if args.jobs > 1: + with ThreadPoolExecutor(max_workers=args.jobs) as executor: + executor.map(run_command_with_logging, commands) + else: + for cmd in commands: + print(cmd) + run_command_with_logging(cmd) + + # --------------------------------------------------------------- + # 5. Check the JSON outputs in ../recordings; remove if out of range + # --------------------------------------------------------------- + totals = [] + if os.path.isdir(args.output_dir): + for filename in os.listdir(args.output_dir): + if filename.endswith(".json"): + file_path = os.path.join(args.output_dir, filename) + with open(file_path, "r") as file: + data = json.load(file) + + placo_data = data.get("Placo", {}) + avg_x_vel = placo_data.get("avg_x_lin_vel", 0) + avg_y_vel = placo_data.get("avg_y_lin_vel", 0) + preset_name = placo_data.get("preset_name", "unknown") + + total_speed = np.sqrt(avg_x_vel**2 + avg_y_vel**2) + + # If the speeds do not fit the indicated preset name, remove + if ( + (preset_name == "slow" and total_speed > slow) + or ( + preset_name == "medium" + and (total_speed <= slow or total_speed > fast) + ) + or (preset_name == "fast" and total_speed <= medium) + ): + os.remove(file_path) + print(f"Deleted {file_path}") + else: + totals.append((total_speed, preset_name)) + else: + print(f"No directory found at {args.output_dir}; skipping file checks.") + totals = sorted(totals, key=numeric_prefix_sort_key) + for speed, preset_name in totals: + print(f"Preset: {preset_name}, Total Speed: {speed:.4f}") + + # --------------------------------------------------------------- + # 6. Optional plotting of anim.npy and sim.npy + # --------------------------------------------------------------- + if args.plot: + import matplotlib.pyplot as plt + + anim_path = os.path.join(SCRIPT_PATH, "anim.npy") + sim_path = os.path.join(SCRIPT_PATH, "sim.npy") + + if os.path.isfile(anim_path) and os.path.isfile(sim_path): + anim = np.load(anim_path) + sim = np.load(sim_path) + print("anim shape:", anim.shape) + print("sim shape:", sim.shape) + + plt.plot(anim[:, 0, 0, 2], label="anim z-pos") + plt.plot(sim[:, 0, 0, 2], label="sim z-pos") + plt.legend() + plt.title("Comparison of anim & sim") + plt.show() + else: + print("anim.npy or sim.npy not found; skipping plot.") + + end_time = time.time() + elapsed_time = end_time - start_time + print(f"Total execution time: {elapsed_time:.2f} seconds") + + +# ----------------------------------------------------------------------------- +# Entrypoint +# ----------------------------------------------------------------------------- +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Generate AMP walking animations") + parser.add_argument( + "--duck", + choices=["go_bdx", "open_duck_mini", "open_duck_mini_v2"], + help="Duck type", + required=True, + ) + parser.add_argument( + "--num", + type=int, + default=100, + help="Number of random motion files to generate.", + ) + parser.add_argument( + "-j", + "--jobs", + nargs="?", # Makes the argument optional + type=int, + const=os.cpu_count(), # Used when -j is provided without a number + default=1, # Default value when -j is not provided + help=( + "Number of parallel jobs. " + "If -j is provided without a number, uses the number of CPU cores available. " + "Default is 1." + ), + ) + parser.add_argument( + "--sweep", action="store_true", help="Sweep through the dx, dy, dtheta values." + ) + parser.add_argument("-v", "--verbose", action="store_true", help="Verbose output") + parser.add_argument( + "--plot", + action="store_true", + help="Optionally plot anim.npy and sim.npy if they exist.", + ) + parser.add_argument( + "--output_dir", + help="Output directory for the recordings", + default=f"{SCRIPT_PATH}/../recordings", + ) + args = parser.parse_args() + main(args) diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/fit_poly.py b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/fit_poly.py new file mode 100644 index 0000000..082abd6 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/fit_poly.py @@ -0,0 +1,78 @@ +import numpy as np +import json +from glob import glob +import os +import argparse +import pickle + +parser = argparse.ArgumentParser() +parser.add_argument("--ref_motion", type=str, default="ref_motion") +args = parser.parse_args() + +all_files = glob(f"{args.ref_motion}/*.json") + + +def fit_ref_motion(file): + data = json.load(open(file)) + Y_all = np.array(data["Frames"]) + period = data["Placo"]["period"] + fps = data["FPS"] + frame_offsets = data["Frame_offset"][0] + startend_double_support_ratio = data["Placo"]["startend_double_support_ratio"] + start_offset = int(startend_double_support_ratio * fps) + nb_steps_in_period = int(period * fps) + _Y = Y_all[start_offset : start_offset + int(nb_steps_in_period)] + joints_pos = _Y[:, frame_offsets["joints_pos"] : frame_offsets["left_toe_pos"]] + joints_vel = _Y[:, frame_offsets["joints_vel"] : frame_offsets["left_toe_vel"]] + foot_contacts = _Y[ + :, frame_offsets["foot_contacts"] : frame_offsets["foot_contacts"] + 2 + ] + base_linear_vel = _Y[ + :, frame_offsets["world_linear_vel"] : frame_offsets["world_angular_vel"] + ] + base_angular_vel = _Y[ + :, frame_offsets["world_angular_vel"] : frame_offsets["joints_vel"] + ] + + Y = np.concatenate( + [joints_pos, joints_vel, foot_contacts, base_linear_vel, base_angular_vel], + axis=1, + ).astype(np.float32) + + # Generate time feature + X = np.linspace(0, 1, Y.shape[0]).reshape(-1, 1).astype(np.float32) # Time variable + + # Polynomial degree + degree = 15 + + # Store coefficients + coefficients = {} + + # ====== Fit Polynomial Regression per Dimension ====== + for dim in range(Y.shape[1]): + coeffs = np.polyfit(X.flatten(), Y[:, dim], degree) + coefficients[f"dim_{dim}"] = list(np.flip(coeffs)) + + ret_data = { + "coefficients": coefficients, + "period": period, + "fps": fps, + "frame_offsets": frame_offsets, + "start_offset": start_offset, + "nb_steps_in_period": nb_steps_in_period, + "startend_double_support_ratio": startend_double_support_ratio, + } + + return ret_data + + +all_coefficients = {} +for file in all_files: + name = os.path.basename(file).strip(".json") + tmp = name.split("_") + name = f"{tmp[1]}_{tmp[2]}_{tmp[3]}" + + all_coefficients[name] = fit_ref_motion(file) + + +pickle.dump(all_coefficients, open("polynomial_coefficients.pkl", "wb")) \ No newline at end of file diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/plot_poly_fit.py b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/plot_poly_fit.py new file mode 100644 index 0000000..bc7ed05 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/plot_poly_fit.py @@ -0,0 +1,162 @@ +import os +import numpy as np +from glob import glob +import json +import matplotlib.pyplot as plt +import argparse +import pickle + +parser = argparse.ArgumentParser() +parser.add_argument( + "--coefficients", + type=str, + default="polynomial_coefficients.pkl", + help="Path to polynomial coefficients file", +) +parser.add_argument("-n", type=int, default=1, help="number of periods to sample") +parser.add_argument("--ref_motion", type=str, default="recordings") +args = parser.parse_args() + +# poly_data = json.load(open(args.coefficients)) +poly_data = pickle.load(open(args.coefficients, "rb")) +# pickle.load(args.coefficients) + +all_ref_files = glob(f"{args.ref_motion}/*.json") + + +def sample_polynomial(time: float, dimension: int, coefficients) -> float: + """ + Evaluate the polynomial at a given time for a specific dimension. + """ + if not (0 <= time <= 1): + raise ValueError("Time must be between 0 and 1.") + + coeffs = coefficients.get(f"dim_{dimension}") + if coeffs is None: + raise ValueError(f"Dimension {dimension} not found in stored coefficients.") + + return sum(c * (time**i) for i, c in enumerate(coeffs)) + + +for file in all_ref_files: + name = os.path.basename(file).strip(".json") + tmp = name.split("_") + name = f"{tmp[1]}_{tmp[2]}_{tmp[3]}" + banner = f"=== {name} ===" + spacer = "=" * len(banner) + print(spacer) + print(banner) + print(spacer) + + period = poly_data[name]["period"] + fps = poly_data[name]["fps"] + frame_offsets = poly_data[name]["frame_offsets"] + startend_double_support_ratio = poly_data[name]["startend_double_support_ratio"] + start_offset = int(startend_double_support_ratio * fps) + nb_steps_in_period = int(period * fps) + + # Load reference motion data + ref_data = json.load(open(file)) + Y_all = np.array(ref_data["Frames"]) + _Y = Y_all[start_offset : start_offset + int(nb_steps_in_period) * args.n] + joints_pos = _Y[:, frame_offsets["joints_pos"] : frame_offsets["left_toe_pos"]] + joints_vel = _Y[:, frame_offsets["joints_vel"] : frame_offsets["left_toe_vel"]] + foot_contacts = _Y[ + :, frame_offsets["foot_contacts"] : frame_offsets["foot_contacts"] + 2 + ] + base_linear_vel = _Y[ + :, frame_offsets["world_linear_vel"] : frame_offsets["world_angular_vel"] + ] + base_angular_vel = _Y[ + :, frame_offsets["world_angular_vel"] : frame_offsets["joints_vel"] + ] + Y = np.concatenate( + [joints_pos, joints_vel, foot_contacts, base_linear_vel, base_angular_vel], + axis=1, + ) + + dimensions_names = [ + "pos left_hip_yaw", + "pos left_hip_roll", + "pos left_hip_pitch", + "pos left_knee", + "pos left_ankle", + "pos neck_pitch", + "pos head_pitch", + "pos head_yaw", + "pos head_roll", + "pos left_antenna", + "pos right_antenna", + "pos right_hip_yaw", + "pos right_hip_roll", + "pos right_hip_pitch", + "pos right_knee", + "pos right_ankle", + "vel left_hip_yaw", + "vel left_hip_roll", + "vel left_hip_pitch", + "vel left_knee", + "vel left_ankle", + "vel neck_pitch", + "vel head_pitch", + "vel head_yaw", + "vel head_roll", + "vel left_antenna", + "vel right_antenna", + "vel right_hip_yaw", + "vel right_hip_roll", + "vel right_hip_pitch", + "vel right_knee", + "vel right_ankle", + "foot_contacts left", + "foot_contacts right", + "base_linear_vel x", + "base_linear_vel y", + "base_linear_vel z", + "base_angular_vel x", + "base_angular_vel y", + "base_angular_vel z", + ] + nb_dim = Y.shape[1] + + # Generate time feature + X = np.linspace(0, 1, Y.shape[0]).reshape(-1, 1) # Time variable + + # Get coefficients for this motion + coefficients = poly_data[name]["coefficients"] + + dimensions = [] + + # ====== Plot Fitting for Each Dimension ====== + for dimension in range(nb_dim): + poly_samples = [] + ref_samples = [] + # times = X.flatten() # Use original time values for accurate mapping + times = np.arange(len(Y)) % nb_steps_in_period / nb_steps_in_period + + for i, t in enumerate(times): + if i >= Y.shape[0]: + break # Prevent out-of-bounds errors + + poly_sample = sample_polynomial(t, dimension, coefficients) + ref = Y[i, dimension] + + poly_samples.append(poly_sample) + ref_samples.append(ref) + dimensions.append((dimension, ref_samples, poly_samples)) + + # ====== Plotting ====== + nb_rows = int(np.sqrt(nb_dim)) + nb_cols = int(np.ceil(nb_dim / nb_rows)) + + fig, axs = plt.subplots(nb_rows, nb_cols, figsize=(12, 8)) + axs = axs.flatten() + + for i, (dim, ref, poly) in enumerate(dimensions): + ax = axs[i] + ax.plot(ref, label="Original Data", alpha=0.5) + ax.plot(poly, label="Polynomial Fit", color="red") + ax.set_title(f"{dimensions_names[dim]}") + + plt.tight_layout() + plt.show() diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/replay_motion.py b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/replay_motion.py new file mode 100644 index 0000000..44a3bba --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/replay_motion.py @@ -0,0 +1,87 @@ +import argparse +import json +import time + +import FramesViewer.utils as fv_utils +import numpy as np +from FramesViewer.viewer import Viewer +from matplotlib import pyplot as plt +from scipy.spatial.transform import Rotation as R + +parser = argparse.ArgumentParser() +parser.add_argument("-f", "--file", type=str, required=True) +parser.add_argument( + "--hardware", + action="store_true", + help="use AMP_for_hardware format. If false, use IsaacGymEnvs format", +) +args = parser.parse_args() + +fv = Viewer() +fv.start() + +episode = json.load(open(args.file)) + +frame_duration = episode["FrameDuration"] + +frames = episode["Frames"] +frame_offsets = episode["Frame_offset"][0] + +root_pos_slice = slice(frame_offsets["root_pos"], frame_offsets["root_quat"]) +root_quat_slice = slice(frame_offsets["root_quat"], frame_offsets["joints_pos"]) + +linear_vel_slice = slice(frame_offsets["world_linear_vel"], frame_offsets["world_angular_vel"]) +angular_vel_slice = slice(frame_offsets["world_angular_vel"], frame_offsets["joints_vel"]) +joint_vels_slice = slice(frame_offsets["joints_vel"], frame_offsets["left_toe_vel"]) + +left_toe_pos_slice = slice(frame_offsets["left_toe_pos"], frame_offsets["right_toe_pos"]) +right_toe_pos_slice = slice(frame_offsets["right_toe_pos"], frame_offsets["world_linear_vel"]) + +if "Debug_info" in episode: + debug = episode["Debug_info"] +else: + debug = None +pose = np.eye(4) +vels = {} +vels["linear_vel"] = [] +vels["angular_vel"] = [] +vels["joint_vels"] = [] +for i, frame in enumerate(frames): + root_position = frame[root_pos_slice] + root_orientation_quat = frame[root_quat_slice] + root_orientation_mat = R.from_quat(root_orientation_quat).as_matrix() + + pose[:3, 3] = root_position + pose[:3, :3] = root_orientation_mat + + fv.pushFrame(pose, "aze") + + vels["linear_vel"].append(frame[linear_vel_slice]) + vels["angular_vel"].append(frame[angular_vel_slice]) + vels["joint_vels"].append(frame[joint_vels_slice]) + + left_toe_pos = np.array(frame[left_toe_pos_slice]) #+ np.array(root_position) + right_toe_pos = np.array(frame[right_toe_pos_slice]) #+ np.array(root_position) + + fv.pushFrame(fv_utils.make_pose(left_toe_pos, [0, 0, 0]), "left_toe") + fv.pushFrame(fv_utils.make_pose(right_toe_pos, [0, 0, 0]), "right_toe") + + time.sleep(frame_duration) + + +# plot vels +x_lin_vel = [vels["linear_vel"][i][0] for i in range(len(frames))] +y_lin_vel = [vels["linear_vel"][i][1] for i in range(len(frames))] +z_lin_vel = [vels["linear_vel"][i][2] for i in range(len(frames))] + +joints_vel = [vels["joint_vels"][i] for i in range(len(frames))] +angular_vel_x = [vels["angular_vel"][i][0] for i in range(len(frames))] +angular_vel_y = [vels["angular_vel"][i][1] for i in range(len(frames))] +angular_vel_z = [vels["angular_vel"][i][2] for i in range(len(frames))] + +plt.plot(angular_vel_x, label="angular_vel_x") +plt.plot(angular_vel_y, label="angular_vel_y") +plt.plot(angular_vel_z, label="angular_vel_z") + +plt.legend() +plt.show() diff --git a/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/replay_sweep.py b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/replay_sweep.py new file mode 100644 index 0000000..c21b453 --- /dev/null +++ b/Open_Duck_reference_motion_generator-main/Open_Duck_reference_motion_generator-main/scripts/replay_sweep.py @@ -0,0 +1,250 @@ +from FramesViewer.viewer import Viewer +import time +import argparse +from glob import glob +import os +import numpy as np +import json +from scipy.spatial.transform import Rotation as R + +parser = argparse.ArgumentParser() +parser.add_argument( + "-d", "--directory", type=str, required=True, help="Directory containing the moves" +) +parser.add_argument("-k", action="store_true", default=False) +args = parser.parse_args() + +COMMANDS_RANGE_X = [-0.1, 0.15] +COMMANDS_RANGE_Y = [-0.2, 0.2] +COMMANDS_RANGE_THETA = [-0.5, 0.5] + + +class ReferenceMotion: + def __init__(self, directory): + self.directory = directory + # load all json files except metadata.json + self.json_files = glob(self.directory + "/*.json") + self.data = {} + self.period = None + self.fps = None + self.frame_offsets = None + self.dx_range = [0, 0] + self.dy_range = [0, 0] + self.dtheta_range = [0, 0] + self.dxs = [] + self.dys = [] + self.dthetas = [] + self.data_array = [] + self.process() + # print(self.frame_offsets) + # exit() + + self.slices = {} + self.slices["root_pos"] = slice( + self.frame_offsets["root_pos"], self.frame_offsets["root_quat"] + ) + self.slices["root_quat"] = slice( + self.frame_offsets["root_quat"], self.frame_offsets["joints_pos"] + ) + self.slices["linear_vel"] = slice( + self.frame_offsets["world_linear_vel"], + self.frame_offsets["world_angular_vel"], + ) + self.slices["angular_vel"] = slice( + self.frame_offsets["world_angular_vel"], self.frame_offsets["joints_vel"] + ) + self.slices["joints_pos"] = slice( + self.frame_offsets["joints_pos"], self.frame_offsets["left_toe_pos"] + ) + self.slices["joint_vels"] = slice( + self.frame_offsets["joints_vel"], self.frame_offsets["left_toe_vel"] + ) + self.slices["left_toe_pos"] = slice( + self.frame_offsets["left_toe_pos"], self.frame_offsets["right_toe_pos"] + ) + self.slices["right_toe_pos"] = slice( + self.frame_offsets["right_toe_pos"], self.frame_offsets["world_linear_vel"] + ) + + + def process(self): + for file in self.json_files: + + if self.period is None: + tmp_file = json.load(open(file)) + self.period = tmp_file["Placo"]["period"] + self.fps = tmp_file["FPS"] + self.frame_offsets = tmp_file["Frame_offset"][0] + num_frames = len(tmp_file["Frames"]) + # print(num_frames) + # exit() + + name = os.path.basename(file).strip(".json") + split = name.split("_") + id = float(split[0]) + dx = float(split[1]) + dy = float(split[2]) + dtheta = float(split[3]) + + if dx not in self.dxs: + self.dxs.append(dx) + + if dy not in self.dys: + self.dys.append(dy) + + if dtheta not in self.dthetas: + self.dthetas.append(dtheta) + + self.dx_range = [min(dx, self.dx_range[0]), max(dx, self.dx_range[1])] + self.dy_range = [min(dy, self.dy_range[0]), max(dy, self.dy_range[1])] + self.dtheta_range = [ + min(dtheta, self.dtheta_range[0]), + max(dtheta, self.dtheta_range[1]), + ] + + if dx not in self.data: + self.data[dx]: dict = {} + + if dy not in self.data[dx]: + self.data[dx][dy]: dict = {} + + if dtheta not in self.data[dx][dy]: + self.data[dx][dy][dtheta] = json.load(open(file))["Frames"] + + self.dxs = sorted(self.dxs) + self.dys = sorted(self.dys) + self.dthetas = sorted(self.dthetas) + + print("dx range: ", self.dx_range) + print("dy range: ", self.dy_range) + print("dtheta range: ", self.dtheta_range) + + nb_dx = len(self.dxs) + nb_dy = len(self.dys) + nb_dtheta = len(self.dthetas) + + print("nb dx", nb_dx) + print("nb dy", nb_dy) + print("nb dtheta", nb_dtheta) + + self.data_array = nb_dx * [None] + + for x, dx in enumerate(self.dxs): + self.data_array[x] = nb_dy * [None] + for y, dy in enumerate(self.dys): + self.data_array[x][y] = nb_dtheta * [None] + for t, dtheta in enumerate(self.dthetas): + self.data_array[x][y][t] = self.data[dx][dy][dtheta] + + print(np.array(self.data_array).shape) + # exit() + + def vel_to_index(self, dx, dy, dtheta): + # First, clamp/cap to the ranges + dx = min(max(dx, self.dx_range[0]), self.dx_range[1]) + dy = min(max(dy, self.dy_range[0]), self.dy_range[1]) + dtheta = min(max(dtheta, self.dtheta_range[0]), self.dtheta_range[1]) + + # Find the closest values in self.dxs, self.dys, self.dthetas + ix = np.argmin(np.abs(np.array(self.dxs) - dx)) + iy = np.argmin(np.abs(np.array(self.dys) - dy)) + itheta = np.argmin(np.abs(np.array(self.dthetas) - dtheta)) + + return ix, iy, itheta + + # def get_closest_value(self, value, values): + # return min(values, key=lambda x: abs(x - value)) + + # def get_closest_values(self, dx, dy, dtheta): + # # use vel_to_index to find the closest value in the data_array + + # ix, iy, itheta = self.vel_to_index(dx, dy, dtheta) + + def get_closest_reference_motion(self, dx, dy, dtheta, i): + ix, iy, itheta = self.vel_to_index(dx, dy, dtheta) + print(ix, iy, itheta) + return self.data_array[ix][iy][itheta][i] + + # dx = self.get_closest_value(dx, self.dxs) + # dy = self.get_closest_value(dy, self.dys) + # dtheta = self.get_closest_value(dtheta, self.dthetas) + + # return self.data[dx][dy][dtheta][i] + + +commands = [0.0, 0, 0] + + +def handle_keyboard(): + global commands + keys = pygame.key.get_pressed() + lin_vel_x = 0 + lin_vel_y = 0 + ang_vel = 0 + if keys[pygame.K_z]: + lin_vel_x = COMMANDS_RANGE_X[1] + if keys[pygame.K_s]: + lin_vel_x = COMMANDS_RANGE_X[0] + if keys[pygame.K_q]: + lin_vel_y = COMMANDS_RANGE_Y[1] + if keys[pygame.K_d]: + lin_vel_y = COMMANDS_RANGE_Y[0] + if keys[pygame.K_a]: + ang_vel = COMMANDS_RANGE_THETA[1] + if keys[pygame.K_e]: + ang_vel = COMMANDS_RANGE_THETA[0] + + commands[0] = lin_vel_x + commands[1] = lin_vel_y + commands[2] = ang_vel + + # print(commands) + + pygame.event.pump() # process event queue + + +if __name__ == "__main__": + fv = Viewer() + fv.start() + + if args.k: + import pygame + + pygame.init() + # open a blank pygame window + screen = pygame.display.set_mode((100, 100)) + pygame.display.set_caption("Press arrow keys to move robot") + + RM = ReferenceMotion(args.directory) + i = 0 + nb_steps_per_period = int(RM.fps * RM.period) # *10 # ?? + command = [-0.1, 0, 0] + root_pose = np.eye(4) + left_toe_pose = np.eye(4) + right_toe_pose = np.eye(4) + while True: + + if args.k: + handle_keyboard() + i += 1 + i = i % 450 + ref = RM.get_closest_reference_motion(*commands, i) + + root_position = ref[RM.slices["root_pos"]] + root_orientation_quat = ref[RM.slices["root_quat"]] + root_orientation_mat = R.from_quat(root_orientation_quat).as_matrix() + root_pose[:3, 3] = root_position + root_pose[:3, :3] = root_orientation_mat + + left_toe_pos = np.array(ref[RM.slices["left_toe_pos"]]) + right_toe_pos = np.array(ref[RM.slices["right_toe_pos"]]) + + left_toe_pose[:3, 3] = left_toe_pos + right_toe_pose[:3, 3] = right_toe_pos + + fv.pushFrame(root_pose, "root") + + fv.pushFrame(left_toe_pose, "left_toe") + fv.pushFrame(right_toe_pose, "right_toe") + + time.sleep(1 / RM.fps)