diff --git a/cmvr-es/config/devices/arm/arm.pb.txt b/cmvr-es/config/devices/arm/arm.pb.txt
index 67430cb3..8ba82207 100644
--- a/cmvr-es/config/devices/arm/arm.pb.txt
+++ b/cmvr-es/config/devices/arm/arm.pb.txt
@@ -156,9 +156,15 @@ arm {
motor {
motor_system_id: "ethercat_motors"
- motor_group_ids: "right_arm_ethercat"
+ motor_group_ids: "dual_arm_ethercat"
dof: 7
joint_names: "R_SHOULDER_P"
+ joint_names: "R_SHOULDER_R"
+ joint_names: "R_SHOULDER_Y"
+ joint_names: "R_ELBOW_R"
+ joint_names: "R_WRIST_P"
+ joint_names: "R_WRIST_Y"
+ joint_names: "R_WRIST_R"
upd_freq: 1000
buffer_size: 50
default_vel: 1.0
diff --git a/cmvr-es/config/devices/arm/arm_eyou_left.pb.txt b/cmvr-es/config/devices/arm/arm_eyou_left.pb.txt
new file mode 100644
index 00000000..27fcdd50
--- /dev/null
+++ b/cmvr-es/config/devices/arm/arm_eyou_left.pb.txt
@@ -0,0 +1,152 @@
+arm {
+ robot_arms {
+ id: "eyou_left_arm"
+
+ motor {
+ motor_system_id: "ethercat_motors"
+ motor_group_ids: "dual_arm_ethercat"
+ dof: 7
+ joint_names: "L_SHOULDER_P"
+ joint_names: "L_SHOULDER_R"
+ joint_names: "L_SHOULDER_Y"
+ joint_names: "L_ELBOW_R"
+ joint_names: "L_WRIST_P"
+ joint_names: "L_WRIST_Y"
+ joint_names: "L_WRIST_R"
+ upd_freq: 1000
+ buffer_size: 50
+ default_vel: 1.0
+ default_acc: 2.0
+ }
+
+ kinematics {
+ pinocchio_dls_ik_solver {
+ urdf_path: "model/xiaoyan_description/dual_arm.urdf"
+ base_frame_name: "PELVIS_S"
+ flange_frame_name: "L_WRIST_R_S"
+ tcp_frame_name: "L_FINGER_TIP_FIXED"
+ max_iters: 100
+ pos_eps: 1e-6
+ rot_eps: 1e-6
+ damping: 1e-6
+ joint_limit_policy {
+ limits {
+ enable: true
+ source: JOINT_LIMIT_SOURCE_CUSTOM
+ joints { joint_name: "L_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+ joints { joint_name: "L_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+ joints { joint_name: "L_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+ joints { joint_name: "L_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
+ joints { joint_name: "L_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+ joints { joint_name: "L_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
+ joints { joint_name: "L_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+ }
+ soft_limit {
+ enable: true
+ margin_ratio: 0.01
+ min_margin_rad: 0.01
+ }
+ avoidance {
+ enable: false
+ gain: 0.2
+ margin_ratio: 0.15
+ max_push: 0.25
+ weight: 0.05
+ }
+ }
+ }
+ }
+
+ motion {
+ move_j {
+ toppra_joint_motion_planner {
+ path_type: TOPPRA_PATH_TYPE_QUINTIC
+ sample_period_s: 0.001
+ grid_size: 150
+ high_grid_size: 300
+ }
+ }
+
+ move_l {
+ pinocchio_cartesian_motion_planner {
+ sample_period_s: 0.001
+ position_gain: 4.0
+ rotation_gain: 4.0
+ line_deviation_check {
+ enable: true
+ line_deviation_warn_m: 0.01
+ line_deviation_stop_m: 0.03
+ line_direction_warn_deg: 20.0
+ line_direction_stop_deg: 45.0
+ line_direction_reset_deg: 10.0
+ line_check_min_distance_m: 0.01
+ }
+ joint_continuity_check {
+ enable: true
+ max_joint_delta_rad: 0.05
+ max_joint_velocity_rad_s: 10.0
+ max_joint_acceleration_rad_s2: 5000.0
+ }
+ cartesian_step_feasibility_check {
+ enable: true
+ min_linear_speed_ratio: 0.2
+ max_linear_direction_deviation_deg: 45.0
+ min_angular_speed_ratio: 0.2
+ max_angular_direction_deviation_deg: 45.0
+ min_desired_linear_speed: 1e-4
+ min_desired_angular_speed: 1e-4
+ }
+ }
+ }
+
+ speed_l {
+ pinocchio_cartesian_motion_planner {
+ linear_velocity_max: 0.55
+ linear_acceleration_max: 5.0
+ linear_jerk_max: 10.0
+ angular_velocity_max: 1.0
+ angular_acceleration_max: 5.0
+ angular_jerk_max: 12.0
+ linear_target_replan_threshold: 1e-4
+ angular_target_replan_threshold: 1e-4
+ linear_reverse_cos_threshold: -0.8660254037844386
+ linear_reverse_switch_speed_threshold: 1e-3
+ enforce_joint_acceleration_limits: true
+ line_deviation_check {
+ enable: true
+ line_deviation_warn_m: 0.01
+ line_deviation_stop_m: 0.03
+ line_direction_warn_deg: 20.0
+ line_direction_stop_deg: 45.0
+ line_direction_reset_deg: 10.0
+ line_check_min_distance_m: 0.01
+ }
+ joint_velocity_check {
+ enable: true
+ max_joint_velocity_rad_s: 30.0
+ max_joint_acceleration_rad_s2: 10000.0
+ }
+ cartesian_velocity_feasibility_check {
+ enable: true
+ min_linear_speed_ratio: 0.2
+ max_linear_direction_deviation_deg: 5.0
+ min_angular_speed_ratio: 0.2
+ max_angular_direction_deviation_deg: 5.0
+ min_desired_linear_speed: 1e-4
+ min_desired_angular_speed: 1e-4
+ }
+ }
+
+ speed_l_controller {
+ cartesian_velocity_controller {
+ control_period_s: 0.001
+ stop_twist_norm: 1e-9
+ stop_command_velocity_norm: 1e-3
+ stop_measured_velocity_norm: 1e-2
+ stop_acceleration: 10
+ }
+ }
+ }
+ }
+ }
+}
diff --git a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt
index 9fda778a..b988b4ea 100644
--- a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt
+++ b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt
@@ -2,7 +2,7 @@ motor {
id: "ethercat_motors"
motor_groups {
- id: "right_arm_ethercat"
+ id: "dual_arm_ethercat"
bus_type: MOTOR_BUS_ETHERCAT
vendor: MOTOR_VENDOR_EYOU
protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
@@ -45,6 +45,13 @@ motor {
slaves { motor_id: 5 alias: 0 position: 4 }
slaves { motor_id: 6 alias: 0 position: 5 }
slaves { motor_id: 7 alias: 0 position: 6 }
+ slaves { motor_id: 8 alias: 0 position: 7 }
+ slaves { motor_id: 9 alias: 0 position: 8 }
+ slaves { motor_id: 10 alias: 0 position: 9 }
+ slaves { motor_id: 11 alias: 0 position: 10 }
+ slaves { motor_id: 12 alias: 0 position: 11 }
+ slaves { motor_id: 13 alias: 0 position: 12 }
+ slaves { motor_id: 14 alias: 0 position: 13 }
}
joint_limits {
@@ -57,6 +64,13 @@ motor {
joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+ joints { joint_name: "L_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+ joints { joint_name: "L_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+ joints { joint_name: "L_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+ joints { joint_name: "L_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
+ joints { joint_name: "L_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+ joints { joint_name: "L_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
+ joints { joint_name: "L_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
}
motors {
@@ -67,6 +81,13 @@ motor {
motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+ motors { id: 8 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+ motors { id: 9 joint_name: "L_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+ motors { id: 10 joint_name: "L_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+ motors { id: 11 joint_name: "L_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+ motors { id: 12 joint_name: "L_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+ motors { id: 13 joint_name: "L_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+ motors { id: 14 joint_name: "L_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
}
diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt
index f01bc267..3d415df7 100644
--- a/cmvr-es/config/manager/device_manager.pb.txt
+++ b/cmvr-es/config/manager/device_manager.pb.txt
@@ -79,7 +79,7 @@ device_manager {
id: "ethercat_motors"
type: DEVICE_TYPE_MOTOR_SYSTEM
config_file: "devices/motor/ethercat_motors.pb.txt"
- enable: false
+ enable: true
}
devices {
@@ -93,7 +93,14 @@ device_manager {
id: "eyou_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/arm.pb.txt"
- enable: false
+ enable: true
+ }
+
+ devices {
+ id: "eyou_left_arm"
+ type: DEVICE_TYPE_ROBOT_ARM
+ config_file: "devices/arm/arm_eyou_left.pb.txt"
+ enable: true
}
devices {
diff --git a/model/xiaoyan_description/dual_arm.urdf b/model/xiaoyan_description/dual_arm.urdf
index 7e7ab687..d033f89c 100644
--- a/model/xiaoyan_description/dual_arm.urdf
+++ b/model/xiaoyan_description/dual_arm.urdf
@@ -274,6 +274,36 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+