| @@ -377,29 +377,29 @@ class Dynamixel: | |||
| ) | |||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | |||
| def set_P(self, motor_id: int, P: int): | |||
| def set_p(self, motor_id: int, p: int): | |||
| """Set the position P gain for the specified servo. | |||
| Args: | |||
| motor_id: ID of the servo to control. | |||
| P: Position P gain value to set. | |||
| motor_id (int): The ID of the servo motor | |||
| p (int): The position P gain value to set | |||
| """ | |||
| dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | |||
| self.portHandler, motor_id, self.POSITION_P, P, | |||
| self.portHandler, motor_id, self.POSITION_P, p, | |||
| ) | |||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | |||
| def set_I(self, motor_id: int, I: int): | |||
| def set_i(self, motor_id: int, i: int): | |||
| """Set the position I gain for the specified servo. | |||
| Args: | |||
| motor_id: ID of the servo to control. | |||
| I: Position I gain value to set. | |||
| motor_id (int): The ID of the servo motor | |||
| i (int): The position I gain value to set | |||
| """ | |||
| dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | |||
| self.portHandler, motor_id, self.POSITION_I, I, | |||
| self.portHandler, motor_id, self.POSITION_I, i, | |||
| ) | |||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | |||
| @@ -1,29 +1,29 @@ | |||
| """TODO: Add docstring.""" | |||
| import numpy as np | |||
| from scipy.spatial.transform import Rotation as R | |||
| from scipy.spatial.transform import Rotation | |||
| def convert_quaternion_to_euler(quat): | |||
| """Convert Quaternion (xyzw) to Euler angles (rpy).""" | |||
| # Normalize | |||
| quat = quat / np.linalg.norm(quat) | |||
| return R.from_quat(quat).as_euler("xyz") | |||
| return Rotation.from_quat(quat).as_euler("xyz") | |||
| def convert_euler_to_quaternion(euler): | |||
| """Convert Euler angles (rpy) to Quaternion (xyzw).""" | |||
| return R.from_euler("xyz", euler).as_quat() | |||
| return Rotation.from_euler("xyz", euler).as_quat() | |||
| def convert_euler_to_rotation_matrix(euler): | |||
| """Convert Euler angles (rpy) to rotation matrix (3x3).""" | |||
| return R.from_euler("xyz", euler).as_matrix() | |||
| return Rotation.from_euler("xyz", euler).as_matrix() | |||
| def convert_rotation_matrix_to_euler(rotmat): | |||
| """Convert rotation matrix (3x3) to Euler angles (rpy).""" | |||
| r = R.from_matrix(rotmat) | |||
| r = Rotation.from_matrix(rotmat) | |||
| return r.as_euler("xyz", degrees=False) | |||
| @@ -32,15 +32,15 @@ def main(): | |||
| break | |||
| if event["type"] == "INPUT" and event["id"] == "text": | |||
| text = event["value"][0].as_py() | |||
| translatedText = argostranslate.translate.translate( | |||
| translated_text = argostranslate.translate.translate( | |||
| text, | |||
| from_code, | |||
| to_code, | |||
| ) | |||
| print(text, flush=True) | |||
| print("translated: " + translatedText, flush=True) | |||
| print("translated: " + translated_text, flush=True) | |||
| node.send_output( | |||
| "text", | |||
| pa.array([translatedText]), | |||
| pa.array([translated_text]), | |||
| {"language": to_code}, | |||
| ) | |||
| @@ -90,26 +90,26 @@ def load_model(): | |||
| """TODO: Add docstring.""" | |||
| from transformers import AutoModelForSpeechSeq2Seq, AutoProcessor, pipeline | |||
| MODEL_NAME_OR_PATH = os.getenv("MODEL_NAME_OR_PATH", DEFAULT_PATH) | |||
| model_name_or_path = os.getenv("MODEL_NAME_OR_PATH", DEFAULT_PATH) | |||
| if bool(os.getenv("USE_MODELSCOPE_HUB") in ["True", "true"]): | |||
| from modelscope import snapshot_download | |||
| if not Path(MODEL_NAME_OR_PATH).exists(): | |||
| MODEL_NAME_OR_PATH = snapshot_download(MODEL_NAME_OR_PATH) | |||
| if not Path(model_name_or_path).exists(): | |||
| model_name_or_path = snapshot_download(model_name_or_path) | |||
| device = "cuda:0" if torch.cuda.is_available() else "cpu" | |||
| torch_dtype = torch.float16 if torch.cuda.is_available() else torch.float32 | |||
| model = AutoModelForSpeechSeq2Seq.from_pretrained( | |||
| MODEL_NAME_OR_PATH, | |||
| model_name_or_path, | |||
| torch_dtype=torch_dtype, | |||
| low_cpu_mem_usage=True, | |||
| use_safetensors=True, | |||
| ) | |||
| model.to(device) | |||
| processor = AutoProcessor.from_pretrained(MODEL_NAME_OR_PATH) | |||
| processor = AutoProcessor.from_pretrained(model_name_or_path) | |||
| return pipeline( | |||
| "automatic-speech-recognition", | |||
| model=model, | |||
| @@ -5,9 +5,9 @@ import os | |||
| import numpy as np | |||
| import pyarrow as pa | |||
| import torch | |||
| import torchvision.transforms as T | |||
| from dora import Node | |||
| from PIL import Image | |||
| from torchvision import transforms | |||
| from torchvision.transforms.functional import InterpolationMode | |||
| from transformers import AutoModel, AutoTokenizer | |||
| @@ -17,13 +17,13 @@ IMAGENET_STD = (0.229, 0.224, 0.225) | |||
| def build_transform(input_size): | |||
| """TODO: Add docstring.""" | |||
| MEAN, STD = IMAGENET_MEAN, IMAGENET_STD | |||
| return T.Compose( | |||
| mean, std = IMAGENET_MEAN, IMAGENET_STD | |||
| return transforms.Compose( | |||
| [ | |||
| T.Lambda(lambda img: img.convert("RGB") if img.mode != "RGB" else img), | |||
| T.Resize((input_size, input_size), interpolation=InterpolationMode.BICUBIC), | |||
| T.ToTensor(), | |||
| T.Normalize(mean=MEAN, std=STD), | |||
| transforms.Lambda(lambda img: img.convert("RGB") if img.mode != "RGB" else img), | |||
| transforms.Resize((input_size, input_size), interpolation=InterpolationMode.BICUBIC), | |||
| transforms.ToTensor(), | |||
| transforms.Normalize(mean=mean, std=std), | |||
| ], | |||
| ) | |||
| @@ -22,33 +22,33 @@ magma_dir = current_dir.parent / "Magma" / "magma" | |||
| def load_magma_models(): | |||
| """TODO: Add docstring.""" | |||
| DEFAULT_PATH = str(magma_dir.parent / "checkpoints" / "Magma-8B") | |||
| if not os.path.exists(DEFAULT_PATH): | |||
| DEFAULT_PATH = str(magma_dir.parent) | |||
| if not os.path.exists(DEFAULT_PATH): | |||
| default_path = str(magma_dir.parent / "checkpoints" / "Magma-8B") | |||
| if not os.path.exists(default_path): | |||
| default_path = str(magma_dir.parent) | |||
| if not os.path.exists(default_path): | |||
| logger.warning( | |||
| "Warning: Magma submodule not found, falling back to HuggingFace version", | |||
| ) | |||
| DEFAULT_PATH = "microsoft/Magma-8B" | |||
| default_path = "microsoft/Magma-8B" | |||
| MODEL_NAME_OR_PATH = os.getenv("MODEL_NAME_OR_PATH", DEFAULT_PATH) | |||
| logger.info(f"Loading Magma model from: {MODEL_NAME_OR_PATH}") | |||
| model_name_or_path = os.getenv("MODEL_NAME_OR_PATH", default_path) | |||
| logger.info(f"Loading Magma model from: {model_name_or_path}") | |||
| try: | |||
| model = AutoModelForCausalLM.from_pretrained( | |||
| MODEL_NAME_OR_PATH, | |||
| model_name_or_path, | |||
| trust_remote_code=True, | |||
| torch_dtype=torch.bfloat16, | |||
| device_map="auto", | |||
| ) | |||
| processor = AutoProcessor.from_pretrained( | |||
| MODEL_NAME_OR_PATH, trust_remote_code=True, | |||
| model_name_or_path, trust_remote_code=True, | |||
| ) | |||
| except Exception as e: | |||
| logger.error(f"Failed to load model: {e}") | |||
| raise | |||
| return model, processor, MODEL_NAME_OR_PATH | |||
| return model, processor, model_name_or_path | |||
| model, processor, MODEL_NAME_OR_PATH = load_magma_models() | |||
| @@ -47,8 +47,8 @@ def enable_fun(piper: C_PiperInterface): | |||
| def main(): | |||
| """TODO: Add docstring.""" | |||
| elapsed_time = time.time() | |||
| CAN_BUS = os.getenv("CAN_BUS", "") | |||
| piper = C_PiperInterface(CAN_BUS) | |||
| can_bus = os.getenv("CAN_BUS", "") | |||
| piper = C_PiperInterface(can_bus) | |||
| piper.ConnectPort() | |||
| if not TEACH_MODE: | |||
| @@ -14,8 +14,8 @@ RUNNER_CI = True if os.getenv("CI") == "true" else False | |||
| def main(): | |||
| """TODO: Add docstring.""" | |||
| FLIP = os.getenv("FLIP", "") | |||
| DEVICE_SERIAL = os.getenv("DEVICE_SERIAL", "") | |||
| flip = os.getenv("FLIP", "") | |||
| device_serial = os.getenv("DEVICE_SERIAL", "") | |||
| image_height = int(os.getenv("IMAGE_HEIGHT", "480")) | |||
| image_width = int(os.getenv("IMAGE_WIDTH", "640")) | |||
| encoding = os.getenv("ENCODING", "rgb8") | |||
| @@ -26,15 +26,15 @@ def main(): | |||
| # Serial list | |||
| serials = [device.get_info(rs.camera_info.serial_number) for device in devices] | |||
| if DEVICE_SERIAL and (DEVICE_SERIAL in serials): | |||
| if device_serial and (device_serial in serials): | |||
| raise ConnectionError( | |||
| f"Device with serial {DEVICE_SERIAL} not found within: {serials}.", | |||
| f"Device with serial {device_serial} not found within: {serials}.", | |||
| ) | |||
| pipeline = rs.pipeline() | |||
| config = rs.config() | |||
| config.enable_device(DEVICE_SERIAL) | |||
| config.enable_device(device_serial) | |||
| config.enable_stream(rs.stream.color, image_width, image_height, rs.format.rgb8, 30) | |||
| config.enable_stream(rs.stream.depth, image_width, image_height) | |||
| @@ -79,11 +79,11 @@ def main(): | |||
| ## Change rgb to bgr | |||
| if FLIP == "VERTICAL": | |||
| if flip == "VERTICAL": | |||
| frame = cv2.flip(frame, 0) | |||
| elif FLIP == "HORIZONTAL": | |||
| elif flip == "HORIZONTAL": | |||
| frame = cv2.flip(frame, 1) | |||
| elif FLIP == "BOTH": | |||
| elif flip == "BOTH": | |||
| frame = cv2.flip(frame, -1) | |||
| # resize the frame | |||
| @@ -81,7 +81,7 @@ def get_language_embeddings(): | |||
| return lang_embeddings.unsqueeze( | |||
| 0, | |||
| ) # Size: (B, L_lang, D) or None, language condition tokens (variable length), dimension D is assumed to be the same as the hidden size. | |||
| ) # Size: (b, L_lang, D) or None, language condition tokens (variable length), dimension D is assumed to be the same as the hidden size. | |||
| def expand2square(pil_img, background_color): | |||
| @@ -144,7 +144,7 @@ def process_image(rgbs_lst, image_processor, vision_encoder): | |||
| def get_states(proprio): | |||
| # suppose you control in 7DOF joint position | |||
| """TODO: Add docstring.""" | |||
| STATE_INDICES = [ | |||
| state_indices = [ | |||
| STATE_VEC_IDX_MAPPING["left_arm_joint_0_pos"], | |||
| STATE_VEC_IDX_MAPPING["left_arm_joint_1_pos"], | |||
| STATE_VEC_IDX_MAPPING["left_arm_joint_2_pos"], | |||
| @@ -161,32 +161,32 @@ def get_states(proprio): | |||
| STATE_VEC_IDX_MAPPING["right_gripper_open"], | |||
| ] | |||
| B, N = 1, 1 # batch size and state history size | |||
| b, n = 1, 1 # batch size and state history size | |||
| states = torch.zeros( | |||
| (B, N, config["model"]["state_token_dim"]), device=DEVICE, dtype=DTYPE, | |||
| (b, n, config["model"]["state_token_dim"]), device=DEVICE, dtype=DTYPE, | |||
| ) | |||
| # suppose you do not have proprio | |||
| # it's kind of tricky, I strongly suggest adding proprio as input and further fine-tuning | |||
| proprio = torch.tensor(proprio, device=DEVICE, dtype=DTYPE).reshape( | |||
| (1, 1, -1), | |||
| ) # B, N = 1, 1 # batch size and state history size | |||
| ) # b, n = 1, 1 # batch size and state history size | |||
| # if you have proprio, you can do like this | |||
| # format like this: [arm_joint_0_pos, arm_joint_1_pos, arm_joint_2_pos, arm_joint_3_pos, arm_joint_4_pos, arm_joint_5_pos, arm_joint_6_pos, gripper_open] | |||
| # proprio = torch.tensor([0, 1, 2, 3, 4, 5, 6, 0.5]).reshape((1, 1, -1)) | |||
| states[:, :, STATE_INDICES] = proprio | |||
| states[:, :, state_indices] = proprio | |||
| state_elem_mask = torch.zeros( | |||
| (1, config["model"]["state_token_dim"]), device=DEVICE, dtype=torch.bool, | |||
| ) | |||
| state_elem_mask[:, STATE_INDICES] = True | |||
| state_elem_mask[:, state_indices] = True | |||
| states, state_elem_mask = ( | |||
| states.to(DEVICE, dtype=DTYPE), | |||
| state_elem_mask.to(DEVICE, dtype=DTYPE), | |||
| ) | |||
| states = states[:, -1:, :] # only use the last state | |||
| return states, state_elem_mask, STATE_INDICES | |||
| return states, state_elem_mask, state_indices | |||
| def main(): | |||
| @@ -172,9 +172,9 @@ def test_dummy_states(): | |||
| # suppose you do not have proprio | |||
| # it's kind of tricky, I strongly suggest adding proprio as input and further fine-tuning | |||
| B, N = 1, 1 # batch size and state history size | |||
| b, n = 1, 1 # batch size and state history size | |||
| states = torch.zeros( | |||
| (B, N, config["model"]["state_token_dim"]), | |||
| (b, n, config["model"]["state_token_dim"]), | |||
| device=DEVICE, | |||
| dtype=DTYPE, | |||
| ) | |||
| @@ -182,10 +182,10 @@ def test_dummy_states(): | |||
| # if you have proprio, you can do like this | |||
| # format like this: [arm_joint_0_pos, arm_joint_1_pos, arm_joint_2_pos, arm_joint_3_pos, arm_joint_4_pos, arm_joint_5_pos, arm_joint_6_pos, gripper_open] | |||
| # proprio = torch.tensor([0, 1, 2, 3, 4, 5, 6, 0.5]).reshape((1, 1, -1)) | |||
| # states[:, :, STATE_INDICES] = proprio | |||
| # states[:, :, state_indices] = proprio | |||
| state_elem_mask = torch.zeros( | |||
| (B, config["model"]["state_token_dim"]), | |||
| (b, config["model"]["state_token_dim"]), | |||
| device=DEVICE, | |||
| dtype=torch.bool, | |||
| ) | |||
| @@ -194,7 +194,7 @@ def test_dummy_states(): | |||
| ) | |||
| # suppose you control in 7DOF joint position | |||
| STATE_INDICES = [ | |||
| state_indices = [ | |||
| STATE_VEC_IDX_MAPPING["arm_joint_0_pos"], | |||
| STATE_VEC_IDX_MAPPING["arm_joint_1_pos"], | |||
| STATE_VEC_IDX_MAPPING["arm_joint_2_pos"], | |||
| @@ -205,7 +205,7 @@ def test_dummy_states(): | |||
| STATE_VEC_IDX_MAPPING["gripper_open"], | |||
| ] | |||
| state_elem_mask[:, STATE_INDICES] = True | |||
| state_elem_mask[:, state_indices] = True | |||
| states, state_elem_mask = ( | |||
| states.to(DEVICE, dtype=DTYPE), | |||
| state_elem_mask.to(DEVICE, dtype=DTYPE), | |||
| @@ -213,7 +213,7 @@ def test_dummy_states(): | |||
| states = states[:, -1:, :] # only use the last state | |||
| pytest.states = states | |||
| pytest.state_elem_mask = state_elem_mask | |||
| pytest.STATE_INDICES = STATE_INDICES | |||
| pytest.state_indices = state_indices | |||
| def test_dummy_input(): | |||
| @@ -227,7 +227,7 @@ def test_dummy_input(): | |||
| image_embeds = pytest.image_embeds | |||
| state_elem_mask = pytest.state_elem_mask | |||
| states = pytest.states | |||
| STATE_INDICES = pytest.STATE_INDICES | |||
| state_indices = pytest.state_indices | |||
| actions = rdt.predict_action( | |||
| lang_tokens=lang_embeddings, | |||
| @@ -242,10 +242,10 @@ def test_dummy_input(): | |||
| ctrl_freqs=torch.tensor([25.0], device=DEVICE), # would this default work? | |||
| ) # (1, chunk_size, 128) | |||
| # select the meaning action via STATE_INDICES | |||
| # select the meaning action via state_indices | |||
| action = actions[ | |||
| :, | |||
| :, | |||
| STATE_INDICES, | |||
| ] # (1, chunk_size, len(STATE_INDICES)) = (1, chunk_size, 7+ 1) | |||
| state_indices, | |||
| ] # (1, chunk_size, len(state_indices)) = (1, chunk_size, 7+ 1) | |||
| print(action) | |||
| @@ -10,7 +10,7 @@ from reachy_sdk.trajectory import goto | |||
| def r_arm_inverse_kinematics(reachy, pose, action) -> list: | |||
| """TODO: Add docstring.""" | |||
| A = np.array( | |||
| a = np.array( | |||
| [ | |||
| [0, 0, -1, pose[0] + action[0]], | |||
| [0, 1, 0, pose[1] + action[1]], | |||
| @@ -18,7 +18,7 @@ def r_arm_inverse_kinematics(reachy, pose, action) -> list: | |||
| [0, 0, 0, 1], | |||
| ], | |||
| ) | |||
| return reachy.r_arm.inverse_kinematics(A) | |||
| return reachy.r_arm.inverse_kinematics(a) | |||
| def happy_antennas(reachy): | |||
| @@ -54,9 +54,9 @@ def main(): | |||
| """TODO: Add docstring.""" | |||
| node = Node() | |||
| ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.24") | |||
| robot_ip = os.getenv("ROBOT_IP", "10.42.0.24") | |||
| reachy = ReachySDK(ROBOT_IP, with_mobile_base=False) | |||
| reachy = ReachySDK(robot_ip, with_mobile_base=False) | |||
| reachy.turn_on("r_arm") | |||
| reachy.turn_on("head") | |||
| @@ -11,14 +11,14 @@ def main(): | |||
| """TODO: Add docstring.""" | |||
| node = Node() | |||
| ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.24") | |||
| CAMERA = os.getenv("CAMERA", "right") | |||
| robot_ip = os.getenv("ROBOT_IP", "10.42.0.24") | |||
| camera = os.getenv("CAMERA", "right") | |||
| reachy = ReachySDK(ROBOT_IP, with_mobile_base=False) | |||
| reachy = ReachySDK(robot_ip, with_mobile_base=False) | |||
| for event in node: | |||
| if event["type"] == "INPUT": | |||
| if CAMERA == "right": | |||
| if camera == "right": | |||
| frame = reachy.right_camera.last_frame | |||
| else: | |||
| frame = reachy.left_camera.last_frame | |||
| @@ -11,10 +11,10 @@ from reachy2_sdk.media.camera import CameraView | |||
| def main(): | |||
| """TODO: Add docstring.""" | |||
| ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80") | |||
| robot_ip = os.getenv("ROBOT_IP", "10.42.0.80") | |||
| for _ in range(10): | |||
| reachy = ReachySDK(ROBOT_IP) | |||
| reachy = ReachySDK(robot_ip) | |||
| try: | |||
| reachy.cameras.teleop.get_frame(view=CameraView.LEFT) | |||
| params = reachy.cameras.depth.get_parameters(view=CameraView.DEPTH) | |||
| @@ -28,7 +28,7 @@ def main(): | |||
| reachy.cameras.teleop.get_frame(view=CameraView.LEFT) | |||
| params = reachy.cameras.depth.get_parameters(view=CameraView.DEPTH) | |||
| height, width, _distortion_model, _D, K, _R, _P = params | |||
| height, width, _distortion_model, _d, k, _r, _p = params | |||
| node = Node() | |||
| @@ -90,8 +90,8 @@ def main(): | |||
| metadata={ | |||
| "width": width, | |||
| "height": height, | |||
| "focal": [int(K[0, 0]), int(K[1, 1])], | |||
| "resolution": [int(K[0, 2]), int(K[1, 2])], | |||
| "focal": [int(k[0, 0]), int(k[1, 1])], | |||
| "resolution": [int(k[0, 2]), int(k[1, 2])], | |||
| }, | |||
| ) | |||
| @@ -9,17 +9,17 @@ from reachy2_sdk import ReachySDK | |||
| def main(): | |||
| """TODO: Add docstring.""" | |||
| ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80") | |||
| robot_ip = os.getenv("ROBOT_IP", "10.42.0.80") | |||
| for _i in range(5): | |||
| reachy = ReachySDK(ROBOT_IP) | |||
| reachy = ReachySDK(robot_ip) | |||
| if reachy.head is not None: | |||
| reachy.head.turn_on() | |||
| reachy.head.goto([0, 0, 0]) | |||
| break | |||
| FOV_H = 107 | |||
| FOV_V = 91 | |||
| fov_h = 107 | |||
| fov_v = 91 | |||
| resolution = [720, 960] | |||
| roll, _pitch, yaw = reachy.head.get_current_positions() | |||
| @@ -37,8 +37,8 @@ def main(): | |||
| x_min + x_max | |||
| ) / 2 - 10 # Deviate a bit to take into account the off centered camera | |||
| y = (3 * y_min + y_max) / 4 | |||
| ry = (x - resolution[1] / 2) * FOV_H / 2 / resolution[1] | |||
| rz = (y - resolution[0] / 2) * FOV_V / 2 / resolution[0] | |||
| ry = (x - resolution[1] / 2) * fov_h / 2 / resolution[1] | |||
| rz = (y - resolution[0] / 2) * fov_v / 2 / resolution[0] | |||
| if np.abs(yaw) > 45 and yaw * -ry > 0: | |||
| reachy.head.cancel_all_goto() | |||
| roll, _pitch, yaw = reachy.head.get_current_positions() | |||
| @@ -7,7 +7,7 @@ import numpy as np | |||
| import pyarrow as pa | |||
| from dora import Node | |||
| from reachy2_sdk import ReachySDK | |||
| from scipy.spatial.transform import Rotation as R | |||
| from scipy.spatial.transform import Rotation | |||
| ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80") | |||
| @@ -26,7 +26,7 @@ l_default_pose = [ | |||
| def l_arm_go_to_mixed_angles(reachy, x, y, z): | |||
| """TODO: Add docstring.""" | |||
| for theta in range(-80, -60, 10): | |||
| r = R.from_euler("zyx", [0, theta, 0], degrees=True) | |||
| r = Rotation.from_euler("zyx", [0, theta, 0], degrees=True) | |||
| transform = np.eye(4) | |||
| transform[:3, :3] = r.as_matrix() | |||
| transform[:3, 3] = [x, y, z] | |||
| @@ -41,7 +41,7 @@ def l_arm_go_to_mixed_angles(reachy, x, y, z): | |||
| ## First try turning left | |||
| pitch = -90 | |||
| r = R.from_euler("ZYX", (-yaw, 0, 0), degrees=True) * R.from_euler( | |||
| r = Rotation.from_euler("ZYX", (-yaw, 0, 0), degrees=True) * Rotation.from_euler( | |||
| "ZYX", | |||
| (0, pitch, 0), | |||
| degrees=True, | |||
| @@ -11,9 +11,9 @@ from reachy2_sdk import ReachySDK | |||
| def main(): | |||
| """TODO: Add docstring.""" | |||
| ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80") | |||
| robot_ip = os.getenv("ROBOT_IP", "10.42.0.80") | |||
| reachy = ReachySDK(ROBOT_IP) | |||
| reachy = ReachySDK(robot_ip) | |||
| if reachy.mobile_base is not None: | |||
| reachy.mobile_base.turn_on() | |||
| @@ -7,7 +7,7 @@ import numpy as np | |||
| import pyarrow as pa | |||
| from dora import Node | |||
| from reachy2_sdk import ReachySDK | |||
| from scipy.spatial.transform import Rotation as R | |||
| from scipy.spatial.transform import Rotation | |||
| ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80") | |||
| @@ -25,7 +25,7 @@ r_default_pose = [ | |||
| def r_arm_go_to_mixed_angles(reachy, x, y, z): | |||
| """TODO: Add docstring.""" | |||
| for theta in range(-80, -60, 10): | |||
| r = R.from_euler("zyx", [0, theta, 0], degrees=True) | |||
| r = Rotation.from_euler("zyx", [0, theta, 0], degrees=True) | |||
| transform = np.eye(4) | |||
| transform[:3, :3] = r.as_matrix() | |||
| transform[:3, 3] = [x, y, z] | |||
| @@ -40,7 +40,7 @@ def r_arm_go_to_mixed_angles(reachy, x, y, z): | |||
| ## First try turning left | |||
| pitch = -90 | |||
| r = R.from_euler("ZYX", (yaw, 0, 0), degrees=True) * R.from_euler( | |||
| r = Rotation.from_euler("ZYX", (yaw, 0, 0), degrees=True) * Rotation.from_euler( | |||
| "ZYX", | |||
| (0, pitch, 0), | |||
| degrees=True, | |||