From 6e85bf8fae1d9df1b1ef2d8f24ad87d84a854fdd Mon Sep 17 00:00:00 2001 From: budzianowski Date: Fri, 1 Nov 2024 15:36:43 -0700 Subject: [PATCH 1/5] functioning setup --- sim/macro.py | 254 ++++++++++++++++++++ sim/resources/stompymicro/robot_fixed.xml | 271 +++++++++++----------- 2 files changed, 385 insertions(+), 140 deletions(-) create mode 100644 sim/macro.py diff --git a/sim/macro.py b/sim/macro.py new file mode 100644 index 00000000..be7019ec --- /dev/null +++ b/sim/macro.py @@ -0,0 +1,254 @@ +""" +python sim/macro.py --embodiment stompymicro +""" +import argparse +import math +import os +from collections import deque +from copy import deepcopy + +import mujoco +import mujoco_viewer +import numpy as np +from scipy.spatial.transform import Rotation as R +from tqdm import tqdm + +from sim.scripts.create_mjcf import load_embodiment + + +class Sim2simCfg: + def __init__( + self, + embodiment, + frame_stack=15, + c_frame_stack=3, + sim_duration=60.0, + dt=0.001, + decimation=10, + cycle_time=0.4, + tau_factor=3, + lin_vel=2.0, + ang_vel=1.0, + dof_pos=1.0, + dof_vel=0.05, + clip_observations=18.0, + clip_actions=18.0, + action_scale=0.25, + ): + self.robot = load_embodiment(embodiment) + + self.num_actions = len(self.robot.all_joints()) + self.frame_stack = frame_stack + self.c_frame_stack = c_frame_stack + self.num_single_obs = 11 + self.num_actions * self.c_frame_stack + self.num_observations = int(self.frame_stack * self.num_single_obs) + + self.sim_duration = sim_duration + self.dt = dt + self.decimation = decimation + + self.cycle_time = cycle_time + + self.tau_factor = tau_factor + self.tau_limit = ( + np.array(list(self.robot.effort().values()) + list(self.robot.effort().values())) * self.tau_factor + ) + self.kps = np.array(list(self.robot.stiffness().values()) + list(self.robot.stiffness().values())) + self.kds = np.array(list(self.robot.damping().values()) + list(self.robot.damping().values())) + + self.lin_vel = lin_vel + self.ang_vel = ang_vel + self.dof_pos = dof_pos + self.dof_vel = dof_vel + + self.clip_observations = clip_observations + self.clip_actions = clip_actions + + self.action_scale = action_scale + + +def pd_control(target_q, q, kp, target_dq, dq, kd, default): + """Calculates torques from position commands""" + return kp * (target_q + default - q) - kd * dq + + +def get_scripted_joint_targets(current_time, cfg, joints): + """ + Returns the target joint positions for the robot at the given time. + + Args: + current_time (float): The current simulation time. + cfg (Sim2simCfg): The simulation configuration. + + Returns: + np.ndarray: The target joint positions. + """ + # Total duration for the scripted behavior + total_duration = 7.0 # seconds + + # Initialize target positions + target_q = np.zeros(cfg.num_actions, dtype=np.double) + + # Define time intervals for different actions + # You can adjust these intervals as needed + t0 = 2.0 # Time to let it fall + t1 = 4.0 # Time to move arms to position + t2 = 6.0 # Time to bend knees + t3 = 8.0 # Time to stand up + + # lie down + # Lie down to arms up (0 to t1) + # breakpoint() + # let it fall + if current_time <= t0: + pass + # elif current_time <= t1: + # progress = current_time / t1 + # # Interpolate arm joints from initial to target position + # # Assuming arm joints are indices 0 and 1 (adjust based on your robot) + # arm_joint_indices = [joints["right_shoulder_pitch"], joints["left_shoulder_pitch"]] # Replace with actual indices + # arm_target_positions = np.deg2rad([90, 90]) # Raise arms up + + # for idx, joint in enumerate(arm_joint_indices): + # target_q[joint] = np.interp(progress, [0, 1], [0, arm_target_positions[idx]]) + + # # Arms up to bend knees (t1 to t2) + # elif current_time <= t2: + # progress = (current_time - t1) / (t2 - t1) + # # Keep arms up + # arm_joint_indices = [0, 1] + # arm_target_positions = np.deg2rad([90, 90]) # Arms remain up + # for idx in arm_joint_indices: + # target_q[idx] = arm_target_positions[idx] + # # Interpolate knee joints to bend position + # # Assuming knee joints are indices 2 and 3 (adjust based on your robot) + # knee_joint_indices = [2, 3] # Replace with actual indices + # knee_bend_positions = np.deg2rad([-90, -90]) # Bend knees + # for idx in knee_joint_indices: + # target_q[idx] = np.interp(progress, [0, 1], [0, knee_bend_positions[idx]]) + + # # Bend knees to stand up (t2 to t3) + # elif current_time <= t3: + # progress = (current_time - t2) / (t3 - t2) + # # Arms remain up + # arm_joint_indices = [0, 1] + # arm_target_positions = np.deg2rad([90, 90]) + # for idx in arm_joint_indices: + # target_q[idx] = arm_target_positions[idx] + # # Knees extend to standing position + # knee_joint_indices = [2, 3] + # knee_bend_positions = np.deg2rad([-90, -90]) + # for idx in knee_joint_indices: + # target_q[idx] = np.interp(progress, [0, 1], [knee_bend_positions[idx], 0]) + + # else: + # # After t3, maintain standing position + # # Arms remain up + # arm_joint_indices = [0, 1] + # arm_target_positions = np.deg2rad([90, 90]) + # for idx in arm_joint_indices: + # target_q[idx] = arm_target_positions[idx] + # # Knees fully extended + # knee_joint_indices = [2, 3] + # for idx in knee_joint_indices: + # target_q[idx] = 0.0 + + return target_q + +def run_mujoco_scripted(cfg): + """ + Run the Mujoco simulation using scripted joint positions. + + Args: + cfg: The configuration object containing simulation settings. + + Returns: + None + """ + model_dir = os.environ.get("MODEL_DIR") + mujoco_model_path = f"{model_dir}/{args.embodiment}/robot_fixed.xml" + + model = mujoco.MjModel.from_xml_path(mujoco_model_path) + model.opt.timestep = cfg.dt + data = mujoco.MjData(model) + + # Initialize default positions + try: + data.qpos = model.keyframe("default").qpos + default = deepcopy(model.keyframe("default").qpos)[-cfg.num_actions:] + print("Default position:", default) + except: + print("No default position found, using zero initialization") + default = np.zeros(cfg.num_actions) + + mujoco.mj_step(model, data) + data.qvel = np.zeros_like(data.qvel) + data.qacc = np.zeros_like(data.qacc) + viewer = mujoco_viewer.MujocoViewer(model, data) + + mujoco.mj_step(model, data) + joints = {} + for ii in range(1, len(data.ctrl)): + joints[data.joint(ii).name] = data.joint(ii).id + + # Initialize target joint positions + target_q = np.zeros((cfg.num_actions), dtype=np.double) + + # Simulation loop + sim_steps = int(cfg.sim_duration / cfg.dt) + for step in tqdm(range(sim_steps), desc="Simulating..."): + current_time = step * cfg.dt + + # Update target_q based on scripted behavior + target_q = get_scripted_joint_targets(current_time, cfg, joints) + + q = data.qpos[-cfg.num_actions:].astype(np.double) + dq = data.qvel[-cfg.num_actions:].astype(np.double) + + target_q = np.zeros((cfg.num_actions), dtype=np.double) + target_dq = np.zeros((cfg.num_actions), dtype=np.double) + + # # Generate PD control + tau = pd_control(target_q, q, cfg.kps, target_dq, dq, cfg.kds, default) + tau = np.clip(tau, -cfg.tau_limit, cfg.tau_limit) + + tau = np.zeros((cfg.num_actions), dtype=np.double) + tau[0] = 10.0 + print("tau:", tau) + print("q:", q) + print("target_q:", target_q) + # breakpoint() + data.ctrl = tau + + mujoco.mj_step(model, data) + viewer.render() + + viewer.close() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Deployment script.") + parser.add_argument("--embodiment", type=str, required=True, help="embodiment") + args = parser.parse_args() + + + if args.embodiment == "stompypro": + cfg = Sim2simCfg( + args.embodiment, + sim_duration=60.0, + dt=0.001, + decimation=10, + cycle_time=0.4, + tau_factor=3.0, + ) + elif args.embodiment == "stompymicro": + cfg = Sim2simCfg( + args.embodiment, + sim_duration=60.0, + dt=0.001, + decimation=10, + cycle_time=0.4, + tau_factor=2., + ) + + run_mujoco_scripted(cfg) diff --git a/sim/resources/stompymicro/robot_fixed.xml b/sim/resources/stompymicro/robot_fixed.xml index f078e8fd..34016432 100644 --- a/sim/resources/stompymicro/robot_fixed.xml +++ b/sim/resources/stompymicro/robot_fixed.xml @@ -1,119 +1,139 @@ - + \ No newline at end of file From a465a70717c9827cf54d57316e0d4e246af5a858 Mon Sep 17 00:00:00 2001 From: budzianowski Date: Fri, 1 Nov 2024 15:37:42 -0700 Subject: [PATCH 2/5] updates --- sim/macro.py | 24 ++--- sim/resources/stompymicro/joints.py | 130 +++++++++++++--------------- 2 files changed, 72 insertions(+), 82 deletions(-) diff --git a/sim/macro.py b/sim/macro.py index be7019ec..c4174ef5 100644 --- a/sim/macro.py +++ b/sim/macro.py @@ -102,15 +102,15 @@ def get_scripted_joint_targets(current_time, cfg, joints): # let it fall if current_time <= t0: pass - # elif current_time <= t1: - # progress = current_time / t1 - # # Interpolate arm joints from initial to target position - # # Assuming arm joints are indices 0 and 1 (adjust based on your robot) - # arm_joint_indices = [joints["right_shoulder_pitch"], joints["left_shoulder_pitch"]] # Replace with actual indices - # arm_target_positions = np.deg2rad([90, 90]) # Raise arms up + elif current_time <= t1: + progress = current_time / t1 + # Interpolate arm joints from initial to target position + # Assuming arm joints are indices 0 and 1 (adjust based on your robot) + arm_joint_indices = [joints["right_shoulder_pitch"], joints["left_shoulder_pitch"]] # Replace with actual indices + arm_target_positions = np.deg2rad([90, 90]) # Raise arms up - # for idx, joint in enumerate(arm_joint_indices): - # target_q[joint] = np.interp(progress, [0, 1], [0, arm_target_positions[idx]]) + for idx, joint in enumerate(arm_joint_indices): + target_q[joint] = np.interp(progress, [0, 1], [0, arm_target_positions[idx]]) # # Arms up to bend knees (t1 to t2) # elif current_time <= t2: @@ -198,13 +198,13 @@ def run_mujoco_scripted(cfg): sim_steps = int(cfg.sim_duration / cfg.dt) for step in tqdm(range(sim_steps), desc="Simulating..."): current_time = step * cfg.dt + q = data.qpos[-cfg.num_actions:].astype(np.double) + dq = data.qvel[-cfg.num_actions:].astype(np.double) + # Update target_q based on scripted behavior target_q = get_scripted_joint_targets(current_time, cfg, joints) - q = data.qpos[-cfg.num_actions:].astype(np.double) - dq = data.qvel[-cfg.num_actions:].astype(np.double) - target_q = np.zeros((cfg.num_actions), dtype=np.double) target_dq = np.zeros((cfg.num_actions), dtype=np.double) @@ -213,7 +213,7 @@ def run_mujoco_scripted(cfg): tau = np.clip(tau, -cfg.tau_limit, cfg.tau_limit) tau = np.zeros((cfg.num_actions), dtype=np.double) - tau[0] = 10.0 + tau[joints["left_shoulder_pitch"]] = 10.0 print("tau:", tau) print("q:", q) print("target_q:", target_q) diff --git a/sim/resources/stompymicro/joints.py b/sim/resources/stompymicro/joints.py index 78156914..4193b772 100644 --- a/sim/resources/stompymicro/joints.py +++ b/sim/resources/stompymicro/joints.py @@ -55,13 +55,13 @@ def __str__(self) -> str: class LeftArm(Node): shoulder_yaw = "left_shoulder_yaw" shoulder_pitch = "left_shoulder_pitch" - elbow_pitch = "left_elbow_yaw" # FIXME: yaw vs pitch + elbow_yaw = "left_elbow_yaw" # FIXME: yaw vs pitch class RightArm(Node): shoulder_yaw = "right_shoulder_yaw" shoulder_pitch = "right_shoulder_pitch" - elbow_pitch = "right_elbow_yaw" # FIXME: yaw vs pitch + elbow_yaw = "right_elbow_yaw" # FIXME: yaw vs pitch class LeftLeg(Node): @@ -89,63 +89,63 @@ class Robot(Node): height = 0.293 rotation = [0, 0, 0.707, 0.707] - # left_arm = LeftArm() - # right_arm = RightArm() + left_arm = LeftArm() + right_arm = RightArm() legs = Legs() @classmethod def default_standing(cls) -> Dict[str, float]: return { # Legs - cls.legs.left.hip_pitch: 0.23, + cls.legs.left.hip_pitch: 0.0, cls.legs.left.hip_yaw: 0, cls.legs.left.hip_roll: 0, - cls.legs.left.knee_pitch: 0.741, - cls.legs.left.ankle_pitch: -0.5, - cls.legs.right.hip_pitch: -0.23, + cls.legs.left.knee_pitch: 0.0, + cls.legs.left.ankle_pitch: -0.0, + cls.legs.right.hip_pitch: -0.0, cls.legs.right.hip_yaw: 0, cls.legs.right.hip_roll: 0, - cls.legs.right.knee_pitch: -0.741, - cls.legs.right.ankle_pitch: 0.5, + cls.legs.right.knee_pitch: 0.0, + cls.legs.right.ankle_pitch: 0.0, # TODO: fixing this for debugging # Arms - # cls.left_arm.shoulder_pitch: 0, - # cls.left_arm.shoulder_yaw: 0, - # cls.left_arm.elbow_pitch: 0, - # cls.right_arm.shoulder_pitch: 0, - # cls.right_arm.shoulder_yaw: 0, - # cls.right_arm.elbow_pitch: 0, + cls.left_arm.shoulder_pitch: 0, + cls.left_arm.shoulder_yaw: 0, + cls.left_arm.elbow_yaw: 0, + cls.right_arm.shoulder_pitch: 0, + cls.right_arm.shoulder_yaw: 0, + cls.right_arm.elbow_yaw: 0, } @classmethod def default_limits(cls) -> Dict[str, Dict[str, float]]: return { - # # left arm - # Robot.left_arm.shoulder_pitch: { - # "lower": -1.5707963, - # "upper": 1.5707963, - # }, - # Robot.left_arm.shoulder_yaw: { - # "lower": -1.5707963, - # "upper": 1.5707963, - # }, - # Robot.left_arm.elbow_pitch: { - # "lower": -1.2217305, - # "upper": 1.2217305, - # }, - # # right arm - # Robot.right_arm.shoulder_pitch: { - # "lower": -1.5707963, - # "upper": 1.5707963, - # }, - # Robot.right_arm.shoulder_yaw: { - # "lower": -1.5707963, - # "upper": 1.5707963, - # }, - # Robot.right_arm.elbow_pitch: { - # "lower": -1.2217305, - # "upper": 1.2217305, - # }, + # left arm + Robot.left_arm.shoulder_pitch: { + "lower": -1.5707963, + "upper": 1.5707963, + }, + Robot.left_arm.shoulder_yaw: { + "lower": -1.5707963, + "upper": 1.5707963, + }, + Robot.left_arm.elbow_yaw: { + "lower": -1.5707963, + "upper": 1.5707963, + }, + # right arm + Robot.right_arm.shoulder_pitch: { + "lower": -1.5707963, + "upper": 1.5707963, + }, + Robot.right_arm.shoulder_yaw: { + "lower": -1.5707963, + "upper": 1.5707963, + }, + Robot.right_arm.elbow_yaw: { + "lower": -1.5707963, + "upper": 1.5707963, + }, # left leg Robot.legs.left.hip_pitch: { "lower": -1.5707963, @@ -194,16 +194,14 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]: @classmethod def stiffness(cls) -> Dict[str, float]: return { - "hip_pitch": 17.681462808698132, - "hip_yaw": 17.681462808698132, - "hip_roll": 17.681462808698132, - "knee_pitch": 17.681462808698132, - "ankle_pitch": 17.681462808698132, - # "shoulder_pitch": 5, - # "shoulder_yaw": 5, - # "shoulder_roll": 5, - # "elbow_pitch": 5, - # "elbow_yaw": 5, + "hip_pitch": 2.5, + "hip_yaw": 2.5, + "hip_roll": 2.5, + "knee_pitch": 2.5, + "ankle_pitch": 2.5, + "shoulder_pitch": 2.5, + "shoulder_yaw": 2.5, + "elbow_yaw": 2.5, } # d_gains @@ -215,11 +213,9 @@ def damping(cls) -> Dict[str, float]: "hip_roll": 0.5354656169048285, "knee_pitch": 0.5354656169048285, "ankle_pitch": 0.5354656169048285, - # "shoulder_pitch": 0.3, - # "shoulder_yaw": 0.3, - # "shoulder_roll": 0.3, - # "elbow_pitch": 0.3, - # "elbow_yaw": 0.3, + "shoulder_pitch": 0.5354656169048285, + "shoulder_yaw": 0.5354656169048285, + "elbow_yaw": 0.5354656169048285, } # pos_limits @@ -231,11 +227,9 @@ def effort(cls) -> Dict[str, float]: "hip_roll": 1, "knee_pitch": 1, "ankle_pitch": 1, - # "shoulder_pitch": 1, - # "shoulder_yaw": 1, - # "shoulder_roll": 1, - # "elbow_pitch": 1, - # "elbow_yaw": 1, + "shoulder_pitch": 1, + "shoulder_yaw": 1, + "elbow_yaw": 1, } # vel_limits @@ -247,11 +241,9 @@ def velocity(cls) -> Dict[str, float]: "hip_roll": 10, "knee_pitch": 10, "ankle_pitch": 10, - # "shoulder_pitch": 10, - # "shoulder_yaw": 10, - # "shoulder_roll": 10, - # "elbow_pitch": 10, - # "elbow_yaw": 10, + "shoulder_pitch": 10, + "shoulder_yaw": 10, + "elbow_yaw": 10, } @classmethod @@ -263,9 +255,7 @@ def friction(cls) -> Dict[str, float]: "hip_roll": 0.05, "knee_pitch": 0.05, "ankle_pitch": 0.05, - # "ankle_pitch": 0.05, - # "elbow_yaw": 0.05, - # "elbow_pitch": 0.05, + "elbow_yaw": 0.05, } From 1db0f11f1b283fcee80917318136303bdbe1bb79 Mon Sep 17 00:00:00 2001 From: budzianowski Date: Fri, 1 Nov 2024 15:53:56 -0700 Subject: [PATCH 3/5] functional but bad signs --- sim/macro.py | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/sim/macro.py b/sim/macro.py index c4174ef5..ea449e31 100644 --- a/sim/macro.py +++ b/sim/macro.py @@ -100,10 +100,12 @@ def get_scripted_joint_targets(current_time, cfg, joints): # Lie down to arms up (0 to t1) # breakpoint() # let it fall + if current_time <= t0: pass - elif current_time <= t1: - progress = current_time / t1 + else:# current_time <= t1: + + progress = (current_time - t0) / (t1 - t0) # Interpolate arm joints from initial to target position # Assuming arm joints are indices 0 and 1 (adjust based on your robot) arm_joint_indices = [joints["right_shoulder_pitch"], joints["left_shoulder_pitch"]] # Replace with actual indices @@ -189,7 +191,7 @@ def run_mujoco_scripted(cfg): mujoco.mj_step(model, data) joints = {} for ii in range(1, len(data.ctrl)): - joints[data.joint(ii).name] = data.joint(ii).id + joints[data.joint(ii).name] = data.joint(ii).id - 1 # Initialize target joint positions target_q = np.zeros((cfg.num_actions), dtype=np.double) @@ -205,15 +207,22 @@ def run_mujoco_scripted(cfg): # Update target_q based on scripted behavior target_q = get_scripted_joint_targets(current_time, cfg, joints) - target_q = np.zeros((cfg.num_actions), dtype=np.double) + # target_q = np.zeros((cfg.num_actions), dtype=np.double) target_dq = np.zeros((cfg.num_actions), dtype=np.double) - + target_q[joints["left_shoulder_yaw"]] = 1.24 # # Generate PD control tau = pd_control(target_q, q, cfg.kps, target_dq, dq, cfg.kds, default) - tau = np.clip(tau, -cfg.tau_limit, cfg.tau_limit) - + # tau = np.clip(tau, -cfg.tau_limit, cfg.tau_limit) + print(joints["left_shoulder_yaw"]) + tau = np.zeros((cfg.num_actions), dtype=np.double) - tau[joints["left_shoulder_pitch"]] = 10.0 + # tau[joints["left_shoulder_pitch"]] = prediction[joints["left_shoulder_pitch"]] + + tau[joints["left_shoulder_pitch"]] = 1.0 # left shoulder pitch + tau[joints["right_shoulder_pitch"]] = -1.0 # left shoulder yaw + # tau[9] = 10.0 # right shoulder yaw + # tau[10] = 10.0 # right elbow yaw + # tau[11] = -2.0 # right hip pitch print("tau:", tau) print("q:", q) print("target_q:", target_q) From 41ae49be50d047ca7b3f8efabda131f550bf8cc8 Mon Sep 17 00:00:00 2001 From: budzianowski Date: Fri, 1 Nov 2024 16:13:30 -0700 Subject: [PATCH 4/5] save --- sim/macro.py | 110 +++++++++++----------------- sim/resources/stompymicro/robot.xml | 107 +++++++++++++++++++++++++++ 2 files changed, 151 insertions(+), 66 deletions(-) create mode 100644 sim/resources/stompymicro/robot.xml diff --git a/sim/macro.py b/sim/macro.py index ea449e31..603b4f23 100644 --- a/sim/macro.py +++ b/sim/macro.py @@ -83,6 +83,7 @@ def get_scripted_joint_targets(current_time, cfg, joints): Returns: np.ndarray: The target joint positions. """ + global save_q # Total duration for the scripted behavior total_duration = 7.0 # seconds @@ -91,10 +92,10 @@ def get_scripted_joint_targets(current_time, cfg, joints): # Define time intervals for different actions # You can adjust these intervals as needed - t0 = 2.0 # Time to let it fall - t1 = 4.0 # Time to move arms to position - t2 = 6.0 # Time to bend knees - t3 = 8.0 # Time to stand up + t0 = .033 # Time to let it fall + t1 = 1.0 # Time to move arms to position + t2 = 2.0 # Time to bend knees + t3 = 3.0 # Time to stand up # lie down # Lie down to arms up (0 to t1) @@ -103,57 +104,50 @@ def get_scripted_joint_targets(current_time, cfg, joints): if current_time <= t0: pass - else:# current_time <= t1: - - progress = (current_time - t0) / (t1 - t0) + elif current_time <= t1: + progress_1 = (current_time - t0) / (t1 - t0) # Interpolate arm joints from initial to target position # Assuming arm joints are indices 0 and 1 (adjust based on your robot) arm_joint_indices = [joints["right_shoulder_pitch"], joints["left_shoulder_pitch"]] # Replace with actual indices - arm_target_positions = np.deg2rad([90, 90]) # Raise arms up + arm_target_positions = np.deg2rad([-90, 90]) # Raise arms up for idx, joint in enumerate(arm_joint_indices): - target_q[joint] = np.interp(progress, [0, 1], [0, arm_target_positions[idx]]) - - # # Arms up to bend knees (t1 to t2) - # elif current_time <= t2: - # progress = (current_time - t1) / (t2 - t1) - # # Keep arms up - # arm_joint_indices = [0, 1] - # arm_target_positions = np.deg2rad([90, 90]) # Arms remain up - # for idx in arm_joint_indices: - # target_q[idx] = arm_target_positions[idx] - # # Interpolate knee joints to bend position - # # Assuming knee joints are indices 2 and 3 (adjust based on your robot) - # knee_joint_indices = [2, 3] # Replace with actual indices - # knee_bend_positions = np.deg2rad([-90, -90]) # Bend knees - # for idx in knee_joint_indices: - # target_q[idx] = np.interp(progress, [0, 1], [0, knee_bend_positions[idx]]) - - # # Bend knees to stand up (t2 to t3) - # elif current_time <= t3: - # progress = (current_time - t2) / (t3 - t2) - # # Arms remain up - # arm_joint_indices = [0, 1] - # arm_target_positions = np.deg2rad([90, 90]) - # for idx in arm_joint_indices: - # target_q[idx] = arm_target_positions[idx] - # # Knees extend to standing position - # knee_joint_indices = [2, 3] - # knee_bend_positions = np.deg2rad([-90, -90]) - # for idx in knee_joint_indices: - # target_q[idx] = np.interp(progress, [0, 1], [knee_bend_positions[idx], 0]) - - # else: - # # After t3, maintain standing position - # # Arms remain up - # arm_joint_indices = [0, 1] - # arm_target_positions = np.deg2rad([90, 90]) - # for idx in arm_joint_indices: - # target_q[idx] = arm_target_positions[idx] - # # Knees fully extended - # knee_joint_indices = [2, 3] - # for idx in knee_joint_indices: - # target_q[idx] = 0.0 + target_q[joint] = np.interp(progress_1, [0, 1], [0, arm_target_positions[idx]]) + + save_q = target_q + # Arms up to bend knees (t1 to t2) + elif current_time <= t2: + target_q = save_q + progress = (current_time - t1) / (t2 - t1) + + knee_joint_indices = [joints["right_knee_pitch"], joints["left_knee_pitch"]] # Replace with actual indices + knee_bend_positions = np.deg2rad([-90, 90]) # Bend knees + for idx, joint in enumerate(knee_joint_indices): + target_q[joint] = np.interp(progress, [0, 1], [0, knee_bend_positions[idx]]) + save_q = target_q + # Bend knees to stand up (t2 to t3) + elif current_time <= t3: + target_q = save_q + progress = (current_time - t2) / (t3 - t2) + + # Knees extend to standing position + pitch_joint_indices = [joints["right_hip_pitch"], joints["left_hip_pitch"]] # Replace with actual indices + pitch_positions = np.deg2rad([-90, 90]) # Bend knees + for idx, joint in enumerate(pitch_joint_indices): + target_q[joint] = np.interp(progress, [0, 1], [0, pitch_positions[idx]]) + save_q = target_q + else: + # After t3, maintain standing position + # Arms remain up + target_q = save_q + arm_joint_indices = [0, 1] + arm_target_positions = np.deg2rad([90, 90]) + for idx in arm_joint_indices: + target_q[idx] = arm_target_positions[idx] + # Knees fully extended + knee_joint_indices = [2, 3] + for idx in knee_joint_indices: + target_q[idx] = 0.0 return target_q @@ -203,30 +197,14 @@ def run_mujoco_scripted(cfg): q = data.qpos[-cfg.num_actions:].astype(np.double) dq = data.qvel[-cfg.num_actions:].astype(np.double) - # Update target_q based on scripted behavior target_q = get_scripted_joint_targets(current_time, cfg, joints) - - # target_q = np.zeros((cfg.num_actions), dtype=np.double) target_dq = np.zeros((cfg.num_actions), dtype=np.double) - target_q[joints["left_shoulder_yaw"]] = 1.24 - # # Generate PD control tau = pd_control(target_q, q, cfg.kps, target_dq, dq, cfg.kds, default) - # tau = np.clip(tau, -cfg.tau_limit, cfg.tau_limit) - print(joints["left_shoulder_yaw"]) - - tau = np.zeros((cfg.num_actions), dtype=np.double) - # tau[joints["left_shoulder_pitch"]] = prediction[joints["left_shoulder_pitch"]] - tau[joints["left_shoulder_pitch"]] = 1.0 # left shoulder pitch - tau[joints["right_shoulder_pitch"]] = -1.0 # left shoulder yaw - # tau[9] = 10.0 # right shoulder yaw - # tau[10] = 10.0 # right elbow yaw - # tau[11] = -2.0 # right hip pitch print("tau:", tau) print("q:", q) print("target_q:", target_q) - # breakpoint() data.ctrl = tau mujoco.mj_step(model, data) diff --git a/sim/resources/stompymicro/robot.xml b/sim/resources/stompymicro/robot.xml new file mode 100644 index 00000000..4f9e8a25 --- /dev/null +++ b/sim/resources/stompymicro/robot.xml @@ -0,0 +1,107 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 915955ca092e624c325f218b939a3ac5e8ce20c7 Mon Sep 17 00:00:00 2001 From: budzianowski Date: Fri, 1 Nov 2024 23:13:27 -0700 Subject: [PATCH 5/5] save the setup --- sim/macro.py | 53 +++-- sim/resources/stompymicro/joints.py | 16 +- .../stompymicro/robot_calibration.xml | 181 ------------------ sim/resources/stompymicro/robot_fixed.xml | 178 +++++++++-------- sim/scripts/create_fixed_torso.py | 5 +- 5 files changed, 128 insertions(+), 305 deletions(-) delete mode 100644 sim/resources/stompymicro/robot_calibration.xml diff --git a/sim/macro.py b/sim/macro.py index 603b4f23..3e2cac05 100644 --- a/sim/macro.py +++ b/sim/macro.py @@ -48,11 +48,6 @@ def __init__( self.decimation = decimation self.cycle_time = cycle_time - - self.tau_factor = tau_factor - self.tau_limit = ( - np.array(list(self.robot.effort().values()) + list(self.robot.effort().values())) * self.tau_factor - ) self.kps = np.array(list(self.robot.stiffness().values()) + list(self.robot.stiffness().values())) self.kds = np.array(list(self.robot.damping().values()) + list(self.robot.damping().values())) @@ -84,8 +79,6 @@ def get_scripted_joint_targets(current_time, cfg, joints): np.ndarray: The target joint positions. """ global save_q - # Total duration for the scripted behavior - total_duration = 7.0 # seconds # Initialize target positions target_q = np.zeros(cfg.num_actions, dtype=np.double) @@ -96,27 +89,42 @@ def get_scripted_joint_targets(current_time, cfg, joints): t1 = 1.0 # Time to move arms to position t2 = 2.0 # Time to bend knees t3 = 3.0 # Time to stand up - + t4 = 4.0 # Time to bend knees # lie down # Lie down to arms up (0 to t1) # breakpoint() # let it fall - + # + # if current_time <= t0: pass + # else: elif current_time <= t1: progress_1 = (current_time - t0) / (t1 - t0) # Interpolate arm joints from initial to target position # Assuming arm joints are indices 0 and 1 (adjust based on your robot) arm_joint_indices = [joints["right_shoulder_pitch"], joints["left_shoulder_pitch"]] # Replace with actual indices - arm_target_positions = np.deg2rad([-90, 90]) # Raise arms up + arm_target_positions = [-1.55, 1.55] # Raise arms up for idx, joint in enumerate(arm_joint_indices): target_q[joint] = np.interp(progress_1, [0, 1], [0, arm_target_positions[idx]]) save_q = target_q - # Arms up to bend knees (t1 to t2) elif current_time <= t2: + # else: + target_q = save_q + progress_2 = (current_time - t1) / (t2 - t1) + # Interpolate arm joints from initial to target position + # Assuming arm joints are indices 0 and 1 (adjust based on your robot) + arm_joint_indices = [joints["right_shoulder_yaw"], joints["left_shoulder_yaw"]] # Replace with actual indices + arm_target_positions = [0.436, -0.436, ] # Raise arms up + + for idx, joint in enumerate(arm_joint_indices): + target_q[joint] = np.interp(progress_2, [0, 1], [0, arm_target_positions[idx]]) + + save_q = target_q + # Arms up to bend knees (t1 to t2) + elif current_time <= t3: target_q = save_q progress = (current_time - t1) / (t2 - t1) @@ -126,28 +134,16 @@ def get_scripted_joint_targets(current_time, cfg, joints): target_q[joint] = np.interp(progress, [0, 1], [0, knee_bend_positions[idx]]) save_q = target_q # Bend knees to stand up (t2 to t3) - elif current_time <= t3: - target_q = save_q - progress = (current_time - t2) / (t3 - t2) + else: + target_q = save_q + progress = (current_time - t3) / (t4 - t3) # Knees extend to standing position pitch_joint_indices = [joints["right_hip_pitch"], joints["left_hip_pitch"]] # Replace with actual indices pitch_positions = np.deg2rad([-90, 90]) # Bend knees for idx, joint in enumerate(pitch_joint_indices): target_q[joint] = np.interp(progress, [0, 1], [0, pitch_positions[idx]]) save_q = target_q - else: - # After t3, maintain standing position - # Arms remain up - target_q = save_q - arm_joint_indices = [0, 1] - arm_target_positions = np.deg2rad([90, 90]) - for idx in arm_joint_indices: - target_q[idx] = arm_target_positions[idx] - # Knees fully extended - knee_joint_indices = [2, 3] - for idx in knee_joint_indices: - target_q[idx] = 0.0 return target_q @@ -201,7 +197,7 @@ def run_mujoco_scripted(cfg): target_q = get_scripted_joint_targets(current_time, cfg, joints) target_dq = np.zeros((cfg.num_actions), dtype=np.double) tau = pd_control(target_q, q, cfg.kps, target_dq, dq, cfg.kds, default) - + print("tau:", tau) print("q:", q) print("target_q:", target_q) @@ -218,7 +214,6 @@ def run_mujoco_scripted(cfg): parser.add_argument("--embodiment", type=str, required=True, help="embodiment") args = parser.parse_args() - if args.embodiment == "stompypro": cfg = Sim2simCfg( args.embodiment, @@ -235,7 +230,7 @@ def run_mujoco_scripted(cfg): dt=0.001, decimation=10, cycle_time=0.4, - tau_factor=2., + tau_factor=10., ) run_mujoco_scripted(cfg) diff --git a/sim/resources/stompymicro/joints.py b/sim/resources/stompymicro/joints.py index 4193b772..4ac62cf2 100644 --- a/sim/resources/stompymicro/joints.py +++ b/sim/resources/stompymicro/joints.py @@ -194,14 +194,14 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]: @classmethod def stiffness(cls) -> Dict[str, float]: return { - "hip_pitch": 2.5, - "hip_yaw": 2.5, - "hip_roll": 2.5, - "knee_pitch": 2.5, - "ankle_pitch": 2.5, - "shoulder_pitch": 2.5, - "shoulder_yaw": 2.5, - "elbow_yaw": 2.5, + "hip_pitch": 5.5, + "hip_yaw": 5.5, + "hip_roll": 5.5, + "knee_pitch": 5.5, + "ankle_pitch": 5.5, + "shoulder_pitch": 5.5, + "shoulder_yaw": 5.5, + "elbow_yaw": 5.5, } # d_gains diff --git a/sim/resources/stompymicro/robot_calibration.xml b/sim/resources/stompymicro/robot_calibration.xml deleted file mode 100644 index 199afdbf..00000000 --- a/sim/resources/stompymicro/robot_calibration.xml +++ /dev/null @@ -1,181 +0,0 @@ - - - - - - - - - - - - diff --git a/sim/resources/stompymicro/robot_fixed.xml b/sim/resources/stompymicro/robot_fixed.xml index 34016432..a025b5aa 100644 --- a/sim/resources/stompymicro/robot_fixed.xml +++ b/sim/resources/stompymicro/robot_fixed.xml @@ -1,7 +1,7 @@ - + @@ -12,20 +12,20 @@ \ No newline at end of file diff --git a/sim/scripts/create_fixed_torso.py b/sim/scripts/create_fixed_torso.py index 7002eb9b..3dde8870 100755 --- a/sim/scripts/create_fixed_torso.py +++ b/sim/scripts/create_fixed_torso.py @@ -6,12 +6,13 @@ from pathlib import Path from sim.scripts.create_mjcf import load_embodiment - +from sim.scripts.create_mjcf import create_mjcf def update_urdf(model_path: str, embodiment: str) -> None: tree = ET.parse(Path(model_path) / "robot.urdf") root = tree.getroot() robot = load_embodiment(embodiment) + print(robot.default_standing()) revolute_joints = set(robot.default_standing().keys()) @@ -57,7 +58,7 @@ def main() -> None: args = parser.parse_args() update_urdf(args.model_path, args.embodiment) - # create_mjcf(Path(args.model_path) / "robot") + create_mjcf(Path(args.model_path) / "robot") if __name__ == "__main__":