From ea6acf905eb3ee22b28a450bbe2d38225cf4e702 Mon Sep 17 00:00:00 2001 From: "Ziwei.He" Date: Tue, 27 May 2025 09:17:24 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BD=8D=E7=BD=AE=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Massage/MassageControl/MassageRobot_nova5.py | 21 +- .../admittance_controller.cpython-39.pyc | Bin 3315 -> 3313 bytes .../position_controller.cpython-39.pyc | Bin 2439 -> 2439 bytes Massage/MassageControl/config/position.yaml | 6 +- .../__pycache__/dobot_nova5.cpython-39.pyc | Bin 22308 -> 22071 bytes .../MassageControl/hardware/dobot_nova5.py | 8 +- logs/MassageRobot_nova5_test.log | 3370 +++++++++++++++++ 7 files changed, 3394 insertions(+), 11 deletions(-) diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py index 335a8db..994e136 100644 --- a/Massage/MassageControl/MassageRobot_nova5.py +++ b/Massage/MassageControl/MassageRobot_nova5.py @@ -274,12 +274,18 @@ class MassageRobot: self.logger.log_blue(f"orientation current: {R.from_quat(quat_rot).as_euler('xyz',degrees=False)}") self.command_rate.precise_sleep() - poistion ,quat_rot = self.arm.get_arm_position() - self.arm_state.desired_position = poistion - self.arm_state.arm_position_command = poistion - self.arm_state.desired_orientation = quat_rot + position ,quat_rot = self.arm.get_arm_position() + # self.arm_state.desired_position = poistion + self.arm_state.arm_position_command = position + # self.arm_state.desired_orientation = quat_rot self.arm_state.arm_orientation_command = quat_rot + ## POSITION CONTROLLER TEST ## + delta_pos = np.array([0.1,0.1,-0.1],dtype=np.float64) + # delta_quat = R.from_euler('xyz', [0, 0, 0], degrees=True).as_quat() + self.arm_state.desired_position = position + delta_pos + self.arm_state.desired_orientation = quat_rot + # 线程启动 self.arm_control_thread.start() ## 赋初始值后控制线程 self.logger.log_info("MassageRobot启动") @@ -371,9 +377,12 @@ if __name__ == "__main__": robot.init_hardwares(ready_pose) time.sleep(3) - robot.controller_manager.switch_controller('admittance') - atexit.register(robot.force_sensor.disconnect) + # robot.controller_manager.switch_controller('admittance') + robot.controller_manager.switch_controller('position') + + + atexit.register(robot.force_sensor.disconnect) robot.arm_state.desired_wrench = np.array([0,0,-0.5,0,0,0]) try: robot_thread = threading.Thread(target=robot.start) diff --git a/Massage/MassageControl/algorithms/__pycache__/admittance_controller.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/admittance_controller.cpython-39.pyc index 0a77fb66b4751f5279fb997809cc06d1fa370d7a..5185c1cb2a3f45968b5e6e05b792314faac53434 100644 GIT binary patch delta 37 qcmew?`B9QLk(ZZ?0SLGxOfueX1adf8UE<49bE4S1H>+};V*&uLO9_4e delta 39 scmew;`B{=Tk(ZZ?0SK-=HO~0B5y;_Y^LB|ZOU;R5_jcK=%yo_l0PA)Os{jB1 diff --git a/Massage/MassageControl/algorithms/__pycache__/position_controller.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/position_controller.cpython-39.pyc index fcde2b9f82bd8c11b961ccce9594be26d9d596a5..ecefec1da54255638fd9c43019934249121be68d 100644 GIT binary patch delta 52 zcmZn{ZWrcGb8h_^J!Xm4j=cnsno12!PX0f>uVF-+9yVJdnyagVFCcnW{2 z-~x$-49!f83_zCTLIy^L6sBMXO@Ym2jHkWoOA~-vxD=q^#lFc;+jkY@7wZ(3CYHd3 zfdWnRyDia2b#?pj2UnPGO}^VUMuEwjLlSwbI30^}6@acuwbGQDd^2=0nDAN-wjg}=HX!C QU<5%fAm(G_VdP^400FsKYybcN delta 521 zcmdnKhH=R{Mx8`nUM>b8xF=|mQP{=6@EF8F20TEH0}vOxF;3L!VVS^K{LPF&T5y2`lrI3~1LdP6Qv_3lS{PC#T^O1fqojbUq|+HvgcmWUhyWp1 zifD@10-03VRJl~CW~POV;tVO`;tUH}qvTT+QstrMD1yvQVG3r@lz0hpm?qOL0hi4x zj0e3K{U&?)%wlBR{M3huk@4l`7rwcS+&`HZ7>d+^>Z(j98wN{HuJ+%~_;Iswz;Py~ zzf7C|1npyFd>_rA1Mk#TofUCGkLJ(L|u&EtahO%)FvJ5R