-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathconfig.yaml
76 lines (74 loc) · 4.03 KB
/
config.yaml
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
embodiments:
stompy_mini:
sides: ["right"]
robot_setup: "right_arm_mini"
urdf_web: "https://raw.githubusercontent.com/kscalelabs/teleop/59796c35863461f8f32a4c21f41903b965cc878e/urdf/stompy_mini/upper_half_assembly_simplified.urdf"
urdf_local: "urdf/stompy_mini/upper_half_assembly_simplified.urdf"
start_pos_trunk_pybullet: [0, 0, 1]
start_eul_trunk_pybullet: [-1.5707963267948966, 0, -1.5707963267948966]
start_pos_trunk_vuer: [0, 1.2, 0]
start_eul_trunk_vuer: [0, 0, 0]
start_pos_eel: [-0.25, -0.25, 0.0]
start_pos_eer: [-0.25, 0.35, 0.0]
start_q:
left shoulder pitch: -1.02
left shoulder yaw: 1.38
left shoulder roll: -3.24
left elbow pitch: 1.2
right shoulder pitch: 3.15
right shoulder yaw: -1.92
right shoulder roll: -1.46
right elbow pitch: 1.32
kinematic_chains:
left_arm: ["left shoulder pitch", "left shoulder yaw", "left shoulder roll", "left elbow pitch"]
left_hand: []
right_arm: ["right shoulder pitch", "right shoulder yaw", "right shoulder roll", "right elbow pitch"]
right_hand: []
stompy:
sides: ["left"]
robot_setup: "left_arm_teleop"
urdf_web: "https://raw.githubusercontent.com/kscalelabs/teleop/f4616b5f117842e5f7eb138b87af31258e1f7484/urdf/stompy/upper_limb_assembly_5_dof_merged_simplified.urdf"
urdf_local: "urdf/stompy/upper_limb_assembly_5_dof_merged_simplified.urdf"
start_pos_trunk_pybullet: [0, 0, 1]
start_eul_trunk_pybullet: [-1.5707963267948966, 0, 2.15]
start_pos_trunk_vuer: [0, 1, 0]
start_eul_trunk_vuer: [-3.141592, -0.68, 0]
start_pos_eel: [-0.35, -0.25, 0.0]
start_pos_eer: [-0.35, 0.25, 0.0]
start_q:
# Trunk
joint_torso_1_rmd_x8_90_mock_1_dof_x8: 0
# Left arm
joint_full_arm_5_dof_1_upper_left_arm_1_rmd_x8_90_mock_1_dof_x8: 0.544
joint_full_arm_5_dof_1_upper_left_arm_1_rmd_x8_90_mock_2_dof_x8: -1.33
joint_full_arm_5_dof_1_upper_left_arm_1_rmd_x4_24_mock_1_dof_x4: 0
joint_full_arm_5_dof_1_upper_left_arm_1_rmd_x4_24_mock_2_dof_x4: 4.8
joint_full_arm_5_dof_1_lower_arm_1_dof_1_rmd_x4_24_mock_2_dof_x4: 1.76
# Left gripper
joint_full_arm_5_dof_1_lower_arm_1_dof_1_hand_1_slider_1: 0.0
joint_full_arm_5_dof_1_lower_arm_1_dof_1_hand_1_slider_2: 0.0
# Right arm
joint_full_arm_5_dof_2_upper_left_arm_1_rmd_x8_90_mock_1_dof_x8: 0.68
joint_full_arm_5_dof_2_upper_left_arm_1_rmd_x8_90_mock_2_dof_x8: 1.24
joint_full_arm_5_dof_2_upper_left_arm_1_rmd_x4_24_mock_1_dof_x4: 0
joint_full_arm_5_dof_2_upper_left_arm_1_rmd_x4_24_mock_2_dof_x4: 3.45
joint_full_arm_5_dof_2_lower_arm_1_dof_1_rmd_x4_24_mock_2_dof_x4: 0
# Right gripper
joint_full_arm_5_dof_2_lower_arm_1_dof_1_hand_1_slider_1: 0.0
joint_full_arm_5_dof_2_lower_arm_1_dof_1_hand_1_slider_2: 0.0
kinematic_chains:
left_arm: ["joint_full_arm_5_dof_1_upper_left_arm_1_rmd_x8_90_mock_1_dof_x8",
"joint_full_arm_5_dof_1_upper_left_arm_1_rmd_x8_90_mock_2_dof_x8",
"joint_full_arm_5_dof_1_upper_left_arm_1_rmd_x4_24_mock_1_dof_x4",
"joint_full_arm_5_dof_1_upper_left_arm_1_rmd_x4_24_mock_2_dof_x4",
"joint_full_arm_5_dof_1_lower_arm_1_dof_1_rmd_x4_24_mock_2_dof_x4"]
left_hand: ["joint_full_arm_5_dof_1_lower_arm_1_dof_1_hand_1_slider_1",
"joint_full_arm_5_dof_1_lower_arm_1_dof_1_hand_1_slider_2"]
right_arm: ["joint_full_arm_5_dof_2_upper_left_arm_1_rmd_x8_90_mock_1_dof_x8",
"joint_full_arm_5_dof_2_upper_left_arm_1_rmd_x8_90_mock_2_dof_x8",
"joint_full_arm_5_dof_2_upper_left_arm_1_rmd_x4_24_mock_1_dof_x4",
"joint_full_arm_5_dof_2_upper_left_arm_1_rmd_x4_24_mock_2_dof_x4",
"joint_full_arm_5_dof_2_lower_arm_1_dof_1_rmd_x4_24_mock_2_dof_x4"]
right_hand: ["joint_full_arm_5_dof_2_lower_arm_1_dof_1_hand_1_slider_1",
"joint_full_arm_5_dof_2_lower_arm_1_dof_1_hand_1_slider_2"]
robot_config_path: "firmware/firmware/robot/teleop_configs.yaml"