├── README.md ├── attitude_control ├── attitude.ttt ├── python │ ├── zmqRemoteApi │ │ ├── __pycache__ │ │ │ └── __init__.cpython-310.pyc │ │ ├── asyncio │ │ │ └── __init__.py │ │ └── __init__.py │ ├── rotation.py │ └── main.py └── readme.md └── multicopter_control ├── bicopter.ttt ├── quadcopter.ttt ├── tricopter.ttt ├── singlecopter.ttt ├── omni-hexcopter.ttt ├── omni-tricopter.ttt ├── python ├── __pycache__ │ ├── rotation.cpython-310.pyc │ ├── impendence.cpython-310.pyc │ ├── kinematics.cpython-310.pyc │ ├── so3_based_ctrl.cpython-310.pyc │ ├── uav_controller.cpython-310.pyc │ └── base_controller.cpython-310.pyc ├── zmqRemoteApi │ ├── __pycache__ │ │ └── __init__.cpython-310.pyc │ ├── asyncio │ │ └── __init__.py │ └── __init__.py ├── base_controller.py ├── rotation.py ├── main.py └── uav_controller.py └── readme.md /README.md: -------------------------------------------------------------------------------- 1 | # learning_robotics 2 | 机器人学习记录 3 | -------------------------------------------------------------------------------- /attitude_control/attitude.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/attitude_control/attitude.ttt -------------------------------------------------------------------------------- /multicopter_control/bicopter.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/multicopter_control/bicopter.ttt -------------------------------------------------------------------------------- /multicopter_control/quadcopter.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/multicopter_control/quadcopter.ttt -------------------------------------------------------------------------------- /multicopter_control/tricopter.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/multicopter_control/tricopter.ttt -------------------------------------------------------------------------------- /multicopter_control/singlecopter.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/multicopter_control/singlecopter.ttt -------------------------------------------------------------------------------- /multicopter_control/omni-hexcopter.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/multicopter_control/omni-hexcopter.ttt -------------------------------------------------------------------------------- /multicopter_control/omni-tricopter.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/multicopter_control/omni-tricopter.ttt -------------------------------------------------------------------------------- /multicopter_control/python/__pycache__/rotation.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/multicopter_control/python/__pycache__/rotation.cpython-310.pyc -------------------------------------------------------------------------------- /multicopter_control/python/__pycache__/impendence.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/multicopter_control/python/__pycache__/impendence.cpython-310.pyc -------------------------------------------------------------------------------- /multicopter_control/python/__pycache__/kinematics.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/multicopter_control/python/__pycache__/kinematics.cpython-310.pyc -------------------------------------------------------------------------------- /multicopter_control/python/__pycache__/so3_based_ctrl.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/multicopter_control/python/__pycache__/so3_based_ctrl.cpython-310.pyc -------------------------------------------------------------------------------- /multicopter_control/python/__pycache__/uav_controller.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/multicopter_control/python/__pycache__/uav_controller.cpython-310.pyc -------------------------------------------------------------------------------- /multicopter_control/python/__pycache__/base_controller.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/multicopter_control/python/__pycache__/base_controller.cpython-310.pyc -------------------------------------------------------------------------------- /attitude_control/python/zmqRemoteApi/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/attitude_control/python/zmqRemoteApi/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /multicopter_control/python/zmqRemoteApi/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/learning_robotics/HEAD/multicopter_control/python/zmqRemoteApi/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /attitude_control/readme.md: -------------------------------------------------------------------------------- 1 | 2 | ### 3 | - CoppeliaSim版本：V4.4.0 EDU 4 | - 默认的仿真步长被修改为：0.02s 5 | - python控制程序基于CoppeliaSim提供的zmqRemoteApi实现：https://github.com/CoppeliaRobotics/zmqRemoteApi 6 | - 控制程序依赖：numpy 7 | - 姿态的运动学控制是在底层实现了一个PD控制，貌似API本身不支持直接设定刚体的角速度 8 | 9 | ### 10 | - rotation.py为不同旋转表示法的相关计算。具体可参考下文： 11 | [姿态表示方法--旋转矩阵、欧拉角、四元数和李代数](https://wuyangning.top/2022/10/29/y2022m11/%E5%A7%BF%E6%80%81%E8%A1%A8%E7%A4%BA%E6%96%B9%E6%B3%95/) 12 | - 1 旋转矩阵 13 | - 1.1 绕x,y,z轴的旋转 14 | - 1.2 旋转矩阵的导数 15 | - 2 欧拉角 16 | - 2.1 由旋转矩阵计算欧拉角 17 | - 2.2 欧拉角的导数 18 | - 3 四元数 19 | - 3.1 四元数的运算 20 | - 3.2 四元数旋转变换 21 | - 3.3 四元数、旋转矩阵和欧拉角 22 | - 3.4 四元数的导数 23 | - 4 李群李代数 24 | - 4.1 $SO(3)$和$SE(3)$群 25 | - 4.2 $\mathfrak{so}(3)$和$\mathfrak{se}(3)$李代数 26 | - 4.3 指数映射和对数映射 27 | - 5 总结和比较 28 | - 5.1 旋转表示的目标 29 | - 5.1 旋转表示法的物理含义 30 | - 5.3 旋转表示法的比较 31 | ### 32 | - main.py中实现了不同姿态表示的运动学控制器和动力学控制器。运动学和动力学控制给出了收敛性分析，动力学控制基于反步控制设计方法，具体可参考下文： 33 | [不同姿态表示法的运动学和动力控制](https://wuyangning.top/2022/11/03/y2022m11/%E5%A7%BF%E6%80%81%E6%8E%A7%E5%88%B6/) 34 | - 1 姿态运动学控制 35 | - 1.1 基于欧拉角的运动学控制 36 | - 1.2 基于四元数的运动学控制 37 | - 1.3 基于$SO(3)$的运动学控制 38 | - 2 姿态动力学控制 39 | - 2.1 基于欧拉角的控制 40 | - 2.2 基于四元数的控制 41 | - 2.3 基于$SO(3)$控制 42 | 43 | ### 44 | - 仿真的视频链接： 45 | 46 | ### 47 | > **参考文献**： 48 | 49 | [1] Lynch K M, Park F C. Modern robotics[M]. Cambridge University Press, 2017. 50 | 51 | [2] Chaturvedi N A, Sanyal A K, McClamroch N H. Rigid-body attitude control[J]. IEEE control systems magazine, 2011, 31(3): 30-51. 52 | 53 | [3] https://github.com/Krasjet/quaternion 54 | 55 | [4] 高翔，张涛，"视觉SLAM十四讲"[M]. 电子工业出版社，2017. 56 | 57 | 58 | -------------------------------------------------------------------------------- /multicopter_control/readme.md: -------------------------------------------------------------------------------- 1 | ### 2 | - CoppeliaSim版本：V4.4.0 EDU 3 | - 默认的仿真步长被修改为：0.02s 4 | - python控制程序基于CoppeliaSim提供的zmqRemoteApi实现，依赖numpy 5 | - 位置控制器采用反馈线性化PD控制(带前馈的PD) 6 | - 姿态控制器采用几何反步控制，可参考[刚体姿态控制](https://wuyangning.top/2022/11/03/y2022m11/%E5%A7%BF%E6%80%81%E6%8E%A7%E5%88%B6/) 7 | - 仿真的无人机类型有 8 | - 正X型四旋翼(Quadcopter) 9 | - 倾转三旋翼(Tricopter) 10 | - 倾转双旋翼(Bicopter) 11 | - 矢量共轴双旋翼(Singlecopter) 12 | - 全驱动倾转三旋翼 13 | - 全驱动六旋翼。 14 | 15 | 16 | ### 17 | - uav_controller.py中实现了不同构型的无人机控制，可参考：[不同构型的旋翼无人机控制](https://wuyangning.top/2023/01/27/y2023m01/%E4%B8%8D%E5%90%8C%E6%9E%84%E5%9E%8B%E6%97%8B%E7%BF%BC%E6%97%A0%E4%BA%BA%E6%9C%BA%E7%9A%84%E6%8E%A7%E5%88%B6/) 18 | - base_controller.py中实现了刚体的姿态和位置控制器，使用方法如下： 19 | 20 | **姿态控制器** 21 | ```python 22 | from base_controller import * 23 | import numpy as np 24 | import time 25 | 26 | 27 | dt=0.02 #控制步长 28 | a_ctrl=atti_ctrl(dt) 29 | 30 | #调整控制增益 31 | a_ctrl.k1=np.diag([1.,1.,1.]) 32 | a_ctrl.k2=np.diag([4.,4.,4.]) 33 | a_ctrl.ki=np.diag([2.,2.,2.]) 34 | 35 | end=time.perf_counter() 36 | while True 37 | if time.perf_counter()-end>dt: 38 | 39 | #给定期望姿态轨迹 40 | a_ctrl.Rd=np.diag([1,1,1]) #期望姿态矩阵 41 | a_ctrl.dRd=np.diag([0,0,0]) #期望姿态矩阵的时间导数 42 | 43 | #从传感器获取姿态和角速度 44 | a_ctrl.R=IMU() #期望姿态 45 | a_ctrl.w=Gyro()# 机体角速度 46 | 47 | #计算控制器 48 | tau=a_ctrl.compute() 49 | 50 | end=time.perf_counter() 51 | 52 | ``` 53 | **位置控制器** 54 | ```python 55 | from base_controller import * 56 | import numpy as np 57 | import time 58 | 59 | 60 | dt=0.02 #控制步长 61 | p_ctrl=posi_ctrl(dt) 62 | 63 | #调整控制增益 64 | p_ctrl.kp=np.diag([1.,1.,1.]) 65 | p_ctrl.kd=np.diag([4.,4.,4.]) 66 | p_ctrl.ki=np.diag([2.,2.,2.]) 67 | 68 | end=time.perf_counter() 69 | while True 70 | if time.perf_counter()-end>dt: 71 | 72 | #给定期望位置轨迹 73 | p_ctrl.pd=np.array([0,0,0]) #期望位置 74 | p_ctrl.dpd=np.array[0,0,0]) #期望速度 75 | p_ctrl.ddpd=np.array[0,0,0]) #期望加速度 76 | 77 | #从感器获取位置和速度 78 | p_ctrl.p=Positin() # 绝对位置 79 | p_ctrl.v=Velocity()# 绝对速度 80 | 81 | #计算控制器 82 | f=p_ctrl.compute() 83 | 84 | end=time.perf_counter() 85 | 86 | ``` 87 | -------------------------------------------------------------------------------- /multicopter_control/python/base_controller.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import rotation as rot 3 | 4 | class atti_ctrl: 5 | def __init__(self,dt) -> None: 6 | self.dt=dt #控制步长 7 | self.J=0.1*np.identity(3) #转动惯量 8 | 9 | 10 | #控制增益 11 | self.k1=np.diag([4,4,3]) 12 | self.k2=np.diag([10,10,5]) 13 | self.ki=np.diag([0,0,0]) 14 | 15 | #期望姿态 16 | self.Rd=np.identity(3) #期望姿态矩阵 17 | self.dRd=np.identity(3) #期望姿态矩阵的导数 18 | self.last_wd=np.array([0,0,0],dtype=float) #上一个时刻的期望姿角速度 19 | 20 | #积分项 21 | self.hatd=np.array([0,0,0],dtype=float) 22 | 23 | #状态量 24 | self.R=np.identity(3) #当前姿态矩阵 25 | self.w=np.array([0,0,0],dtype=float) #当前机体角速度 26 | 27 | def set_target(self,Rd,dRd): 28 | self.Rd=Rd 29 | self.dRd=dRd 30 | 31 | 32 | def update_state(self,R,w): 33 | self.R=R 34 | self.w=w 35 | 36 | 37 | def compute(self): 38 | #基于so3和反步技术的姿态控制器 39 | 40 | wd=rot.so3_vee(self.Rd.T@self.dRd) 41 | dwd=(wd-self.last_wd)/self.dt 42 | 43 | self.last_wd=wd.copy() 44 | 45 | 46 | #姿态控制器 47 | Re=self.Rd.T@self.R 48 | phi_e=rot.so3_vee(Re-Re.T) 49 | dRe=Re@rot.sk(self.w)-rot.sk(wd)@Re 50 | dphi_e=rot.so3_vee(dRe-dRe.T) 51 | Omega_e=dRe.T@wd#+Re.T@dwd #可根据情况忽略高阶项以减少噪声 52 | 53 | z=self.w+self.k1@phi_e-Re.T@wd 54 | self.hatd=self.hatd+self.dt*self.ki@z 55 | tau=np.cross(self.w,self.J@self.w)+self.J@(-self.k2@z-phi_e-self.k1@dphi_e+Omega_e)-self.hatd 56 | 57 | return tau 58 | 59 | 60 | class posi_ctrl: 61 | def __init__(self,dt) -> None: 62 | self.dt=dt #控制步长 63 | self.M=1 # 无人机质量 64 | self.ge3=np.array([0,0,9.81],dtype=float) #重力加速度 65 | 66 | #控制增益 67 | self.kp=np.diag([6,6,6]) 68 | self.kd=np.diag([3.,3.,3.]) 69 | self.ki=np.diag([0,0,0]) 70 | 71 | #期望轨迹 72 | self.pd=np.array([0,0,1],dtype=float) #期望位置轨迹 73 | self.dpd=np.array([0,0,0],dtype=float) #期望速度轨迹 74 | self.ddpd=np.array([0,0,0],dtype=float) #期望加速度轨迹 75 | 76 | #积分项 77 | self.hatd=np.array([0,0,0],dtype=float) 78 | 79 | #状态量 80 | self.p=np.array([0,0,0],dtype=float) #当前位置 81 | self.v=np.array([0,0,0],dtype=float) #当前速度 82 | 83 | 84 | def set_target(self,pd,dpd,ddpd): 85 | self.pd=pd 86 | self.dpd=dpd 87 | self.ddpd=ddpd 88 | 89 | 90 | def update_state(self,p,v): 91 | self.p=p 92 | self.v=v 93 | 94 | 95 | def compute(self): 96 | 97 | #反馈线性化+PD控制 98 | e=self.p-self.pd 99 | de=self.v-self.dpd 100 | self.hatd=self.hatd+self.dt*self.ki@e 101 | Rf=self.M*(-self.kp@e-self.kd@de+self.ge3+self.ddpd)-self.hatd 102 | 103 | return Rf 104 | 105 | class pid_ctrl: 106 | def __init__(self,dt): 107 | #控制增益 108 | self.kp=1.0 109 | self.ki=0.0 110 | self.kd=0.0 111 | self.dt=dt 112 | 113 | #积分项 114 | self.inte=0.0 115 | 116 | def compute(self,e,de): 117 | #e=x-xd 误差的定义为状态减期望 118 | self.inte=self.inte+self.ki*e*self.dt 119 | u=-self.kp*e-self.kd*de-self.inte 120 | return u 121 | 122 | -------------------------------------------------------------------------------- /attitude_control/python/zmqRemoteApi/asyncio/__init__.py: -------------------------------------------------------------------------------- 1 | """CoppeliaSim's Remote API client.""" 2 | 3 | import asyncio 4 | import sys 5 | import os 6 | import uuid 7 | 8 | from contextlib import contextmanager 9 | 10 | import cbor 11 | 12 | import zmq 13 | import zmq.asyncio 14 | 15 | 16 | if sys.platform == 'win32' and sys.version_info >= (3, 8, 0): 17 | if isinstance(asyncio.get_event_loop_policy(), asyncio.windows_events.WindowsProactorEventLoopPolicy): 18 | print(""" 19 | 20 | WARNING: on Windows and Python 3.8+, `asyncio` might not work properly; in case, add the following before `asyncio.run(...)`: 21 | 22 | if sys.platform == 'win32' and sys.version_info >= (3, 8, 0): 23 | asyncio.set_event_loop_policy(asyncio.WindowsSelectorEventLoopPolicy()) 24 | 25 | """) 26 | 27 | 28 | def b64(b): 29 | import base64 30 | return base64.b64encode(b).decode('ascii') 31 | 32 | 33 | class RemoteAPIClient: 34 | """Client to connect to CoppeliaSim's ZMQ Remote API.""" 35 | 36 | def __init__(self, host='localhost', port=23000, cntport=None, *, verbose=None): 37 | """Create client and connect to the ZMQ Remote API server.""" 38 | self.verbose = int(os.environ.get('VERBOSE', '0')) if verbose is None else verbose 39 | self.host, self.port, self.cntport = host, port, cntport or port + 1 40 | self.cntsocket = None 41 | self.uuid=str(uuid.uuid4()) 42 | # multiple sockets will be created for multiple concurrent requests, as needed 43 | self.sockets = [] 44 | 45 | async def __aenter__(self): 46 | """Add one socket to the pool.""" 47 | self.context = zmq.asyncio.Context() 48 | self.cntsocket = self.context.socket(zmq.SUB) 49 | self.cntsocket.setsockopt(zmq.SUBSCRIBE, b'') 50 | self.cntsocket.setsockopt(zmq.CONFLATE, 1) 51 | self.cntsocket.connect(f'tcp://{self.host}:{self.cntport}') 52 | return self 53 | 54 | async def __aexit__(self, *excinfo): 55 | """Disconnect and destroy client.""" 56 | for socket in self.sockets: 57 | socket.close() 58 | self.cntsocket.close() 59 | self.context.term() 60 | 61 | @contextmanager 62 | def _socket(self): 63 | if not self.sockets: 64 | socket = self.context.socket(zmq.REQ) 65 | socket.connect(f'tcp://{self.host}:{self.port}') 66 | if self.verbose > 0: 67 | print('Added a new socket:', socket) 68 | else: 69 | socket = self.sockets.pop() 70 | if self.verbose > 0: 71 | print('Reusing existing socket:', socket) 72 | try: 73 | yield socket 74 | finally: 75 | self.sockets.append(socket) 76 | 77 | async def _send(self, socket, req): 78 | if self.verbose > 0: 79 | print('Sending:', req, socket) 80 | rawReq = cbor.dumps(req) 81 | if self.verbose > 1: 82 | print(f'Sending raw len={len(rawReq)}, base64={b64(rawReq)}') 83 | await socket.send(rawReq) 84 | 85 | async def _recv(self, socket): 86 | rawResp = await socket.recv() 87 | if self.verbose > 1: 88 | print(f'Received raw len={len(rawResp)}, base64={b64(rawResp)}') 89 | resp = cbor.loads(rawResp) 90 | if self.verbose > 0: 91 | print('Received:', resp, socket) 92 | return resp 93 | 94 | def _process_response(self, resp): 95 | if not resp.get('success', False): 96 | raise Exception(resp.get('error')) 97 | ret = resp['ret'] 98 | if len(ret) == 1: 99 | return ret[0] 100 | if len(ret) > 1: 101 | return tuple(ret) 102 | 103 | async def call(self, func, args): 104 | """Call function with specified arguments.""" 105 | with self._socket() as socket: 106 | await self._send(socket, {'func': func, 'args': args}) 107 | return self._process_response(await self._recv(socket)) 108 | 109 | async def getObject(self, name, _info=None): 110 | """Retrieve remote object from server.""" 111 | ret = type(name, (), {}) 112 | if not _info: 113 | _info = await self.call('zmqRemoteApi.info', [name]) 114 | for k, v in _info.items(): 115 | if not isinstance(v, dict): 116 | raise ValueError('found nondict') 117 | if len(v) == 1 and 'func' in v: 118 | setattr(ret, k, lambda *a, func=f'{name}.{k}': self.call(func, a)) 119 | elif len(v) == 1 and 'const' in v: 120 | setattr(ret, k, v['const']) 121 | else: 122 | setattr(ret, k, self.getObject(f'{name}.{k}', _info=v)) 123 | return ret 124 | 125 | async def setStepping(self, enable=True): 126 | return await self.call('setStepping', [enable,self.uuid]) 127 | 128 | async def step(self, *, wait=True): 129 | await self.getStepCount(False) 130 | await self.call('step', [self.uuid]) 131 | await self.getStepCount(wait) 132 | 133 | async def getStepCount(self, wait): 134 | try: 135 | await self.cntsocket.recv(0 if wait else zmq.NOBLOCK) 136 | except zmq.ZMQError: 137 | pass 138 | 139 | 140 | __all__ = ['RemoteAPIClient'] 141 | -------------------------------------------------------------------------------- /multicopter_control/python/zmqRemoteApi/asyncio/__init__.py: -------------------------------------------------------------------------------- 1 | """CoppeliaSim's Remote API client.""" 2 | 3 | import asyncio 4 | import sys 5 | import os 6 | import uuid 7 | 8 | from contextlib import contextmanager 9 | 10 | import cbor 11 | 12 | import zmq 13 | import zmq.asyncio 14 | 15 | 16 | if sys.platform == 'win32' and sys.version_info >= (3, 8, 0): 17 | if isinstance(asyncio.get_event_loop_policy(), asyncio.windows_events.WindowsProactorEventLoopPolicy): 18 | print(""" 19 | 20 | WARNING: on Windows and Python 3.8+, `asyncio` might not work properly; in case, add the following before `asyncio.run(...)`: 21 | 22 | if sys.platform == 'win32' and sys.version_info >= (3, 8, 0): 23 | asyncio.set_event_loop_policy(asyncio.WindowsSelectorEventLoopPolicy()) 24 | 25 | """) 26 | 27 | 28 | def b64(b): 29 | import base64 30 | return base64.b64encode(b).decode('ascii') 31 | 32 | 33 | class RemoteAPIClient: 34 | """Client to connect to CoppeliaSim's ZMQ Remote API.""" 35 | 36 | def __init__(self, host='localhost', port=23000, cntport=None, *, verbose=None): 37 | """Create client and connect to the ZMQ Remote API server.""" 38 | self.verbose = int(os.environ.get('VERBOSE', '0')) if verbose is None else verbose 39 | self.host, self.port, self.cntport = host, port, cntport or port + 1 40 | self.cntsocket = None 41 | self.uuid=str(uuid.uuid4()) 42 | # multiple sockets will be created for multiple concurrent requests, as needed 43 | self.sockets = [] 44 | 45 | async def __aenter__(self): 46 | """Add one socket to the pool.""" 47 | self.context = zmq.asyncio.Context() 48 | self.cntsocket = self.context.socket(zmq.SUB) 49 | self.cntsocket.setsockopt(zmq.SUBSCRIBE, b'') 50 | self.cntsocket.setsockopt(zmq.CONFLATE, 1) 51 | self.cntsocket.connect(f'tcp://{self.host}:{self.cntport}') 52 | return self 53 | 54 | async def __aexit__(self, *excinfo): 55 | """Disconnect and destroy client.""" 56 | for socket in self.sockets: 57 | socket.close() 58 | self.cntsocket.close() 59 | self.context.term() 60 | 61 | @contextmanager 62 | def _socket(self): 63 | if not self.sockets: 64 | socket = self.context.socket(zmq.REQ) 65 | socket.connect(f'tcp://{self.host}:{self.port}') 66 | if self.verbose > 0: 67 | print('Added a new socket:', socket) 68 | else: 69 | socket = self.sockets.pop() 70 | if self.verbose > 0: 71 | print('Reusing existing socket:', socket) 72 | try: 73 | yield socket 74 | finally: 75 | self.sockets.append(socket) 76 | 77 | async def _send(self, socket, req): 78 | if self.verbose > 0: 79 | print('Sending:', req, socket) 80 | rawReq = cbor.dumps(req) 81 | if self.verbose > 1: 82 | print(f'Sending raw len={len(rawReq)}, base64={b64(rawReq)}') 83 | await socket.send(rawReq) 84 | 85 | async def _recv(self, socket): 86 | rawResp = await socket.recv() 87 | if self.verbose > 1: 88 | print(f'Received raw len={len(rawResp)}, base64={b64(rawResp)}') 89 | resp = cbor.loads(rawResp) 90 | if self.verbose > 0: 91 | print('Received:', resp, socket) 92 | return resp 93 | 94 | def _process_response(self, resp): 95 | if not resp.get('success', False): 96 | raise Exception(resp.get('error')) 97 | ret = resp['ret'] 98 | if len(ret) == 1: 99 | return ret[0] 100 | if len(ret) > 1: 101 | return tuple(ret) 102 | 103 | async def call(self, func, args): 104 | """Call function with specified arguments.""" 105 | with self._socket() as socket: 106 | await self._send(socket, {'func': func, 'args': args}) 107 | return self._process_response(await self._recv(socket)) 108 | 109 | async def getObject(self, name, _info=None): 110 | """Retrieve remote object from server.""" 111 | ret = type(name, (), {}) 112 | if not _info: 113 | _info = await self.call('zmqRemoteApi.info', [name]) 114 | for k, v in _info.items(): 115 | if not isinstance(v, dict): 116 | raise ValueError('found nondict') 117 | if len(v) == 1 and 'func' in v: 118 | setattr(ret, k, lambda *a, func=f'{name}.{k}': self.call(func, a)) 119 | elif len(v) == 1 and 'const' in v: 120 | setattr(ret, k, v['const']) 121 | else: 122 | setattr(ret, k, self.getObject(f'{name}.{k}', _info=v)) 123 | return ret 124 | 125 | async def setStepping(self, enable=True): 126 | return await self.call('setStepping', [enable,self.uuid]) 127 | 128 | async def step(self, *, wait=True): 129 | await self.getStepCount(False) 130 | await self.call('step', [self.uuid]) 131 | await self.getStepCount(wait) 132 | 133 | async def getStepCount(self, wait): 134 | try: 135 | await self.cntsocket.recv(0 if wait else zmq.NOBLOCK) 136 | except zmq.ZMQError: 137 | pass 138 | 139 | 140 | __all__ = ['RemoteAPIClient'] 141 | -------------------------------------------------------------------------------- /attitude_control/python/rotation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | #绕x轴旋转的旋转矩阵 4 | def rot_x(theta): 5 | return np.array([[1,0,0], 6 | [0,np.cos(theta),-np.sin(theta)], 7 | [0,np.sin(theta),np.cos(theta)]],dtype=np.float64) 8 | 9 | #绕y轴旋转的旋转矩阵 10 | def rot_y(theta): 11 | return np.array([[np.cos(theta),0,np.sin(theta)], 12 | [0,1,0], 13 | [-np.sin(theta),0,np.cos(theta)]],dtype=np.float64) 14 | 15 | #绕z轴旋转的旋转矩阵 16 | def rot_z(theta): 17 | return np.array([[np.cos(theta),-np.sin(theta),0], 18 | [np.sin(theta),np.cos(theta),0], 19 | [0,0,1]],dtype=np.float64) 20 | 21 | #三维向量对应的反对称矩阵 22 | def sk(x): 23 | return np.array([[0,-x[2],x[1]], 24 | [x[2],0,-x[0]], 25 | [-x[1],x[0],0]],dtype=np.float64) 26 | 27 | #绕机体坐标系zyx顺序旋转的欧拉角对应的旋转矩阵 28 | #Phi=[phi:x,theta:y,psi:z] 29 | def euler_to_rot(Phi): 30 | return rot_z(Phi[2])@rot_y(Phi[1])@rot_x(Phi[0]) 31 | 32 | #已知旋转矩阵求绕机体坐标系zyx顺序旋转的欧拉角 33 | def rot_to_euler(R): 34 | #返回结果是欧拉角的两组解 35 | #Phi=[phi:x,theta:y,psi:z] 36 | Phi1=np.array([0,0,0],dtype=np.float64) 37 | Phi2=np.array([0,0,0],dtype=np.float64) 38 | 39 | if abs(R[2,0])==1: 40 | sc=np.sign(R[2,0]) 41 | 42 | #奇异点，psi可为任意值，有无穷多解，这里设成0和pi/2 43 | Phi1[2]=0 44 | Phi2[2]=np.pi/2 45 | 46 | Phi1[1]=sc*np.pi/2 47 | Phi2[1]=sc*np.pi/2 48 | 49 | Phi1[0]=sc*Phi1[2]+np.arctan2(sc*R[0,1],sc*R[0,2]) 50 | Phi2[0]=sc*Phi2[2]+np.arctan2(sc*R[0,1],sc*R[0,2]) 51 | else: 52 | Phi1[1]=-np.arcsin(R[2,0]) 53 | Phi2[1]=np.pi-Phi1[1] 54 | 55 | Phi1[0]=np.arctan2(R[2,1]/np.cos(Phi1[1]),R[2,2]/np.cos(Phi1[1])) 56 | Phi2[0]=np.arctan2(R[2,1]/np.cos(Phi2[1]),R[2,2]/np.cos(Phi2[1])) 57 | 58 | Phi1[2]=np.arctan2(R[1,0]/np.cos(Phi1[1]),R[0,0]/np.cos(Phi1[1])) 59 | Phi2[2]=np.arctan2(R[1,0]/np.cos(Phi2[1]),R[0,0]/np.cos(Phi2[1])) 60 | 61 | return Phi1,Phi2 62 | 63 | #四元数乘法 64 | def qtimes(q1,q2): 65 | s1=q1[0] 66 | v1=q1[1:] 67 | Q=np.r_[np.array([np.r_[s1,-v1]]),np.c_[v1,s1*np.identity(3)+sk(v1)]] 68 | return Q@q2 69 | 70 | #四元数对应的旋转矩阵 71 | def quat_to_rot(q): 72 | q=np.array(q) 73 | v=q[1:].reshape(3,1) 74 | A=q[0]*np.identity(3)+sk(q[1:]) 75 | return v@v.T+A@A 76 | 77 | #旋转矩阵转四元数 78 | def rot_to_quat(R): 79 | q=np.array([1,0,0,0],dtype=float) 80 | q[0]=np.sqrt(np.abs(R[0,0]+R[1,1]+R[2,2]+1))/2 81 | q[1]=np.sign(R[2,1]-R[1,2])*np.sqrt(np.abs(R[0,0]-R[1,1]-R[2,2]+1))/2 82 | q[2]=np.sign(R[0,2]-R[2,0])*np.sqrt(np.abs(-R[0,0]+R[1,1]-R[2,2]+1))/2 83 | q[3]=np.sign(R[1,0]-R[0,1])*np.sqrt(np.abs(-R[0,0]-R[1,1]+R[2,2]+1))/2 84 | return q 85 | 86 | #绕机体坐标系zyx顺序旋转的欧拉角对应的四元数 87 | def euler_to_quat1(Phi): 88 | qz=np.array([np.cos(Phi[2]/2),0,0,np.sin(Phi[2]/2)],dtype=np.float64) 89 | qy=np.array([np.cos(Phi[1]/2),0,np.sin(Phi[1]/2),0],dtype=np.float64) 90 | qx=np.array([np.cos(Phi[0]/2),np.sin(Phi[0]/2),0,0],dtype=np.float64) 91 | return qtimes(qtimes(qz,qy),qx) 92 | 93 | #欧拉角转四元数的展开形式 94 | def euler_to_quat(Phi): 95 | cx=np.cos(Phi[0]/2) 96 | sx=np.sin(Phi[0]/2) 97 | cy=np.cos(Phi[1]/2) 98 | sy=np.sin(Phi[1]/2) 99 | cz=np.cos(Phi[2]/2) 100 | sz=np.sin(Phi[2]/2) 101 | 102 | return np.array([cx*cy*cz+sx*sy*sz, 103 | sx*cy*cz-cx*sy*sz, 104 | cx*sy*cz+sx*cy*sz, 105 | cx*cy*sz-sx*sy*cz],dtype=np.float64) 106 | 107 | 108 | 109 | #so(3)向量到反对称矩阵的映射 110 | def so3_wedge(x): 111 | return np.array([[0,-x[2],x[1]], 112 | [x[2],0,-x[0]], 113 | [-x[1],x[0],0]],dtype=np.float64) 114 | #反对称矩阵到so(3)向量的映射 115 | def so3_vee(X): 116 | return np.array([(X[2,1]-X[1,2])/2,(X[0,2]-X[2,0])/2,(X[1,0]-X[0,1])/2],dtype=np.float64) 117 | 118 | #SO(3)空间的指数映射 119 | #so(3)向量对应的旋转矩阵 120 | def so3_exp_map(phi): 121 | theta=np.linalg.norm(phi) 122 | a=np.array(phi)/theta 123 | ac=a.reshape(3,1) 124 | return np.cos(theta)*np.identity(3)+(1-np.cos(theta))*ac@ac.T+np.sin(theta)*so3_wedge(a) 125 | 126 | #SO(3)空间的对数映射 127 | #旋转矩阵对应的so(3)向量 128 | def so3_ln_map(R): 129 | theta=np.arccos((np.trace(R)-1)*0.5) 130 | a=(0.5/np.sin(theta))*so3_vee(R-R.T) 131 | return theta*a 132 | 133 | #se(3)向量到R^4*4矩阵 134 | def se3_wedge(xi): 135 | rho=np.array(xi[0:3]) 136 | X=so3_wedge(xi[3:]) 137 | return np.r_[np.c_[X,rho],np.array([[0,0,0,0]])] 138 | 139 | #R^4*4矩阵到se(3)向量 140 | def se3_vee(T): 141 | x=so3_vee(T[0:3,0:3]) 142 | rho=T[0:3,3:].reshape(1,3)[0] 143 | return np.r_[rho,x] 144 | 145 | def se3_exp_map(xi): 146 | rho=np.array(xi[0:3]) 147 | theta=np.linalg.norm(xi[3:]) 148 | a=np.array(xi[3:])/theta 149 | aa_T=a.reshape(3,1)@a.reshape(3,1).T 150 | wedge_a=so3_wedge(a) 151 | exp_phi=np.cos(theta)*np.identity(3)+(1-np.cos(theta))*aa_T+np.sin(theta)*wedge_a 152 | J=(np.sin(theta)/theta)*np.identity(3)+(1-np.sin(theta)/theta)*aa_T+((1-np.cos(theta))/theta)*wedge_a 153 | return np.r_[np.c_[exp_phi,J@rho],np.array([[0,0,0,1]])] 154 | 155 | def se3_ln_map(T): 156 | R=T[0:3,0:3] 157 | t=T[0:3,3:].reshape(1,3)[0] 158 | theta=np.arccos((np.trace(R)-1)*0.5) 159 | a=(0.5/np.sin(theta))*so3_vee(R-R.T) 160 | ac=a.reshape(3,1) 161 | J=(np.sin(theta)/theta)*np.identity(3)+(1-np.sin(theta)/theta)*ac@ac.T+((1-np.cos(theta))/theta)*so3_wedge(a) 162 | return np.r_[np.linalg.inv(J)@t,theta*a] 163 | 164 | 165 | -------------------------------------------------------------------------------- /multicopter_control/python/rotation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | #绕x轴旋转的旋转矩阵 4 | def rot_x(theta): 5 | return np.array([[1,0,0], 6 | [0,np.cos(theta),-np.sin(theta)], 7 | [0,np.sin(theta),np.cos(theta)]],dtype=np.float64) 8 | 9 | #绕y轴旋转的旋转矩阵 10 | def rot_y(theta): 11 | return np.array([[np.cos(theta),0,np.sin(theta)], 12 | [0,1,0], 13 | [-np.sin(theta),0,np.cos(theta)]],dtype=np.float64) 14 | 15 | #绕z轴旋转的旋转矩阵 16 | def rot_z(theta): 17 | return np.array([[np.cos(theta),-np.sin(theta),0], 18 | [np.sin(theta),np.cos(theta),0], 19 | [0,0,1]],dtype=np.float64) 20 | 21 | #三维向量对应的反对称矩阵 22 | def sk(x): 23 | return np.array([[0,-x[2],x[1]], 24 | [x[2],0,-x[0]], 25 | [-x[1],x[0],0]],dtype=np.float64) 26 | 27 | #绕机体坐标系zyx顺序旋转的欧拉角对应的旋转矩阵 28 | #Phi=[phi:x,theta:y,psi:z] 29 | def euler_to_rot(Phi): 30 | return rot_z(Phi[2])@rot_y(Phi[1])@rot_x(Phi[0]) 31 | 32 | #已知旋转矩阵求绕机体坐标系zyx顺序旋转的欧拉角 33 | def rot_to_euler(R): 34 | #返回结果是欧拉角的两组解 35 | #Phi=[phi:x,theta:y,psi:z] 36 | Phi1=np.array([0,0,0],dtype=np.float64) 37 | Phi2=np.array([0,0,0],dtype=np.float64) 38 | 39 | if abs(R[2,0])==1: 40 | sc=np.sign(R[2,0]) 41 | 42 | #奇异点，psi可为任意值，有无穷多解，这里设成0和pi/2 43 | Phi1[2]=0 44 | Phi2[2]=np.pi/2 45 | 46 | Phi1[1]=sc*np.pi/2 47 | Phi2[1]=sc*np.pi/2 48 | 49 | Phi1[0]=sc*Phi1[2]+np.arctan2(sc*R[0,1],sc*R[0,2]) 50 | Phi2[0]=sc*Phi2[2]+np.arctan2(sc*R[0,1],sc*R[0,2]) 51 | else: 52 | Phi1[1]=-np.arcsin(R[2,0]) 53 | Phi2[1]=np.pi-Phi1[1] 54 | 55 | Phi1[0]=np.arctan2(R[2,1]/np.cos(Phi1[1]),R[2,2]/np.cos(Phi1[1])) 56 | Phi2[0]=np.arctan2(R[2,1]/np.cos(Phi2[1]),R[2,2]/np.cos(Phi2[1])) 57 | 58 | Phi1[2]=np.arctan2(R[1,0]/np.cos(Phi1[1]),R[0,0]/np.cos(Phi1[1])) 59 | Phi2[2]=np.arctan2(R[1,0]/np.cos(Phi2[1]),R[0,0]/np.cos(Phi2[1])) 60 | 61 | return Phi1,Phi2 62 | 63 | #四元数乘法 64 | def qtimes(q1,q2): 65 | s1=q1[0] 66 | v1=q1[1:] 67 | Q=np.r_[np.array([np.r_[s1,-v1]]),np.c_[v1,s1*np.identity(3)+sk(v1)]] 68 | return Q@q2 69 | 70 | #四元数对应的旋转矩阵 71 | def quat_to_rot(q): 72 | q=np.array(q) 73 | v=q[1:].reshape(3,1) 74 | A=q[0]*np.identity(3)+sk(q[1:]) 75 | return v@v.T+A@A 76 | 77 | #旋转矩阵转四元数 78 | def rot_to_quat(R): 79 | q=np.array([1,0,0,0],dtype=float) 80 | q[0]=np.sqrt(np.abs(R[0,0]+R[1,1]+R[2,2]+1))/2 81 | q[1]=np.sign(R[2,1]-R[1,2])*np.sqrt(np.abs(R[0,0]-R[1,1]-R[2,2]+1))/2 82 | q[2]=np.sign(R[0,2]-R[2,0])*np.sqrt(np.abs(-R[0,0]+R[1,1]-R[2,2]+1))/2 83 | q[3]=np.sign(R[1,0]-R[0,1])*np.sqrt(np.abs(-R[0,0]-R[1,1]+R[2,2]+1))/2 84 | return q 85 | 86 | #绕机体坐标系zyx顺序旋转的欧拉角对应的四元数 87 | def euler_to_quat1(Phi): 88 | qz=np.array([np.cos(Phi[2]/2),0,0,np.sin(Phi[2]/2)],dtype=np.float64) 89 | qy=np.array([np.cos(Phi[1]/2),0,np.sin(Phi[1]/2),0],dtype=np.float64) 90 | qx=np.array([np.cos(Phi[0]/2),np.sin(Phi[0]/2),0,0],dtype=np.float64) 91 | return qtimes(qtimes(qz,qy),qx) 92 | 93 | #欧拉角转四元数的展开形式 94 | def euler_to_quat(Phi): 95 | cx=np.cos(Phi[0]/2) 96 | sx=np.sin(Phi[0]/2) 97 | cy=np.cos(Phi[1]/2) 98 | sy=np.sin(Phi[1]/2) 99 | cz=np.cos(Phi[2]/2) 100 | sz=np.sin(Phi[2]/2) 101 | 102 | return np.array([cx*cy*cz+sx*sy*sz, 103 | sx*cy*cz-cx*sy*sz, 104 | cx*sy*cz+sx*cy*sz, 105 | cx*cy*sz-sx*sy*cz],dtype=np.float64) 106 | 107 | 108 | 109 | #so(3)向量到反对称矩阵的映射 110 | def so3_wedge(x): 111 | return np.array([[0,-x[2],x[1]], 112 | [x[2],0,-x[0]], 113 | [-x[1],x[0],0]],dtype=np.float64) 114 | #反对称矩阵到so(3)向量的映射 115 | def so3_vee(X): 116 | return np.array([(X[2,1]-X[1,2])/2,(X[0,2]-X[2,0])/2,(X[1,0]-X[0,1])/2],dtype=np.float64) 117 | 118 | #SO(3)空间的指数映射 119 | #so(3)向量对应的旋转矩阵 120 | def so3_exp_map(phi): 121 | theta=np.linalg.norm(phi) 122 | a=np.array(phi)/theta 123 | ac=a.reshape(3,1) 124 | return np.cos(theta)*np.identity(3)+(1-np.cos(theta))*ac@ac.T+np.sin(theta)*so3_wedge(a) 125 | 126 | #SO(3)空间的对数映射 127 | #旋转矩阵对应的so(3)向量 128 | def so3_ln_map(R): 129 | theta=np.arccos((np.trace(R)-1)*0.5) 130 | a=(0.5/np.sin(theta))*so3_vee(R-R.T) 131 | return theta*a 132 | 133 | #se(3)向量到R^4*4矩阵 134 | def se3_wedge(xi): 135 | rho=np.array(xi[0:3]) 136 | X=so3_wedge(xi[3:]) 137 | return np.r_[np.c_[X,rho],np.array([[0,0,0,0]])] 138 | 139 | #R^4*4矩阵到se(3)向量 140 | def se3_vee(T): 141 | x=so3_vee(T[0:3,0:3]) 142 | rho=T[0:3,3:].reshape(1,3)[0] 143 | return np.r_[rho,x] 144 | 145 | def se3_exp_map(xi): 146 | rho=np.array(xi[0:3]) 147 | theta=np.linalg.norm(xi[3:]) 148 | a=np.array(xi[3:])/theta 149 | aa_T=a.reshape(3,1)@a.reshape(3,1).T 150 | wedge_a=so3_wedge(a) 151 | exp_phi=np.cos(theta)*np.identity(3)+(1-np.cos(theta))*aa_T+np.sin(theta)*wedge_a 152 | J=(np.sin(theta)/theta)*np.identity(3)+(1-np.sin(theta)/theta)*aa_T+((1-np.cos(theta))/theta)*wedge_a 153 | return np.r_[np.c_[exp_phi,J@rho],np.array([[0,0,0,1]])] 154 | 155 | def se3_ln_map(T): 156 | R=T[0:3,0:3] 157 | t=T[0:3,3:].reshape(1,3)[0] 158 | theta=np.arccos((np.trace(R)-1)*0.5) 159 | a=(0.5/np.sin(theta))*so3_vee(R-R.T) 160 | ac=a.reshape(3,1) 161 | J=(np.sin(theta)/theta)*np.identity(3)+(1-np.sin(theta)/theta)*ac@ac.T+((1-np.cos(theta))/theta)*so3_wedge(a) 162 | return np.r_[np.linalg.inv(J)@t,theta*a] 163 | 164 | 165 | -------------------------------------------------------------------------------- /multicopter_control/python/main.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from uav_controller import* 3 | 4 | try: 5 | from zmqRemoteApi import RemoteAPIClient 6 | except: 7 | print("Can not find zmqRemoteApi lib") 8 | 9 | 10 | client = RemoteAPIClient() 11 | sim = client.getObject('sim') 12 | 13 | UAV=sim.getObject('./UAV') 14 | 15 | 16 | 17 | 18 | 19 | dt=0.02 #控制步长，需要与仿真中的步长保持一致 20 | quad=quadcopter(dt) 21 | tri=tricopter(dt) 22 | bi=bicopter(dt) 23 | single=singlecopter(dt) 24 | omni_tri=omni_tricopter(dt) 25 | omni_hex=omni_hexcopter(dt) 26 | 27 | def run_quadcoper(t): 28 | # 设定无人机期望轨迹 29 | h=1 30 | a=0.1*np.pi 31 | b=0.6*np.pi 32 | pd=np.array([h*np.sin(b*t)*np.cos(a*t), 33 | h*np.sin(b*t)*np.sin(a*t), 34 | h*np.cos(b*t)+2]) #球面螺旋形 35 | dpd=np.array([(h*np.sin(b*(t+dt))*np.cos(a*(t+dt))-h*np.sin(b*t)*np.cos(a*t))/dt, 36 | (h*np.sin(b*(t+dt))*np.sin(a*(t+dt))-h*np.sin(b*t)*np.sin(a*t))/dt, 37 | (h*np.cos(b*(t+dt))-h*np.cos(b*t))/dt]) 38 | ddpd=0*np.array([(h*np.sin(b*(t+2*dt))*np.cos(a*(t+2*dt))-2*h*np.sin(b*(t+dt))*np.cos(a*(t+dt))+h*np.sin(b*t)*np.cos(a*t))/(dt*dt), 39 | (h*np.sin(b*(t+2*dt))*np.sin(a*(t+2*dt))-2*h*np.sin(b*(t+dt))*np.sin(a*(t+dt))+h*np.sin(b*t)*np.sin(a*t))/(dt*dt), 40 | (h*np.cos(b*(t+2*dt))-2*h*np.cos(b*(t+dt))+h*np.cos(b*t))/(dt*dt)]) 41 | yawd=t 42 | 43 | quad.set_target(pd,dpd,ddpd,yawd) 44 | 45 | 46 | #获取无人机状态 47 | p=np.array(sim.getObjectPosition(UAV,-1)) 48 | v,w=sim.getObjectVelocity(UAV) 49 | v=np.array(v) 50 | w=np.array(w) 51 | R=np.array(sim.getObjectMatrix(UAV,-1)).reshape(3,4)[0:3,0:3] 52 | 53 | quad.set_state(p,v,R,w) 54 | 55 | 56 | fi=quad.run() 57 | 58 | sim.setFloatSignal('f1',fi[0]) 59 | sim.setFloatSignal('f2',fi[1]) 60 | sim.setFloatSignal('f3',fi[2]) 61 | sim.setFloatSignal('f4',fi[3]) 62 | 63 | def run_tricoper(t): 64 | # 设定无人机期望轨迹 65 | pd=np.array([np.cos(0.25*t),np.sin(0.25*t),0.5*np.cos(t)+1.5]) 66 | dpd=np.array([-0.25*np.sin(0.25*t),0.25*np.cos(0.25*t),-0.5*np.sin(t)]) 67 | ddpd=np.array([-0.0625*np.cos(0.25*t),-0.0625*np.sin(0.25*t),-0.5*np.cos(t)]) 68 | yawd=0.5*t 69 | 70 | tri.set_target(pd,dpd,ddpd,yawd) 71 | 72 | 73 | #获取无人机状态 74 | p=np.array(sim.getObjectPosition(UAV,-1)) 75 | v,w=sim.getObjectVelocity(UAV) 76 | v=np.array(v) 77 | w=np.array(w) 78 | R=np.array(sim.getObjectMatrix(UAV,-1)).reshape(3,4)[0:3,0:3] 79 | 80 | tri.set_state(p,v,R,w) 81 | 82 | 83 | uc=tri.run() 84 | 85 | sim.setFloatSignal('f1',uc[0]) 86 | sim.setFloatSignal('f2',uc[1]) 87 | sim.setFloatSignal('f3',uc[2]) 88 | sim.setFloatSignal('theta',uc[3]) 89 | 90 | def run_bicoper(t): 91 | 92 | pd=np.array([np.cos(0.25*t),np.sin(0.25*t),0.5*np.cos(t)+1.5]) 93 | dpd=np.array([-0.25*np.sin(0.25*t),0.25*np.cos(0.25*t),-0.5*np.sin(t)]) 94 | ddpd=np.array([-0.0625*np.cos(0.25*t),-0.0625*np.sin(0.25*t),-0.5*np.cos(t)]) 95 | yawd=0.5*t 96 | 97 | bi.set_target(pd,dpd,ddpd,yawd) 98 | 99 | 100 | #获取无人机状态 101 | p=np.array(sim.getObjectPosition(UAV,-1)) 102 | v,w=sim.getObjectVelocity(UAV) 103 | v=np.array(v) 104 | w=np.array(w) 105 | R=np.array(sim.getObjectMatrix(UAV,-1)).reshape(3,4)[0:3,0:3] 106 | 107 | bi.set_state(p,v,R,w) 108 | 109 | uc=bi.run() 110 | 111 | sim.setFloatSignal('f1',uc[0]) 112 | sim.setFloatSignal('f2',uc[1]) 113 | sim.setFloatSignal('theta1',uc[2]) 114 | sim.setFloatSignal('theta2',uc[3]) 115 | 116 | 117 | def run_singlecoper(t): 118 | 119 | pd=np.array([np.cos(0.5*t),np.sin(0.5*t),0.5*np.cos(t)+1.5]) 120 | dpd=np.array([-0.5*np.sin(0.5*t),0.5*np.cos(0.5*t),-0.5*np.sin(t)]) 121 | ddpd=np.array([-0.25*np.cos(0.5*t),-0.25*np.sin(0.5*t),-0.5*np.cos(t)]) 122 | yawd=0 123 | 124 | 125 | 126 | single.set_target(pd,dpd,ddpd,yawd) 127 | 128 | 129 | #获取无人机状态 130 | p=np.array(sim.getObjectPosition(UAV,-1)) 131 | v,w=sim.getObjectVelocity(UAV) 132 | v=np.array(v) 133 | w=np.array(w) 134 | R=np.array(sim.getObjectMatrix(UAV,-1)).reshape(3,4)[0:3,0:3] 135 | 136 | single.set_state(p,v,R,w) 137 | 138 | uc=single.run() 139 | 140 | sim.setFloatSignal('f1',uc[0]) 141 | sim.setFloatSignal('f2',uc[1]) 142 | sim.setFloatSignal('theta1',uc[2]) 143 | sim.setFloatSignal('theta2',uc[3]) 144 | 145 | 146 | def run_omni_tricoper(t): 147 | pd=np.array([np.cos(0.2*t),np.sin(0.2*t),1.5]) 148 | dpd=np.array([-0.2*np.sin(0.2*t),0.2*np.cos(0.2*t),-0.0*np.sin(t)]) 149 | ddpd=np.array([-0.04*np.cos(0.2*t),-0.04*np.sin(0.2*t),-0.0*np.cos(t)]) 150 | 151 | Rd=rot.euler_to_rot([0.3*np.sin(0.2*t),0.3*np.cos(0.2*t),0]) 152 | dRd=(rot.euler_to_rot([0.3*np.sin(0.2*(t+dt)),0.3*np.cos(0.2*(t+dt)),0]) 153 | -rot.euler_to_rot([0.3*np.sin(0.2*t),0.3*np.cos(0.2*t),0]))/dt 154 | 155 | omni_tri.set_target(pd,dpd,ddpd,Rd,dRd) 156 | 157 | 158 | #获取无人机状态 159 | p=np.array(sim.getObjectPosition(UAV,-1)) 160 | v,w=sim.getObjectVelocity(UAV) 161 | v=np.array(v) 162 | w=np.array(w) 163 | R=np.array(sim.getObjectMatrix(UAV,-1)).reshape(3,4)[0:3,0:3] 164 | 165 | omni_tri.set_state(p,v,R,w) 166 | 167 | uc=omni_tri.run() 168 | 169 | sim.setFloatSignal('f1',uc[0]) 170 | sim.setFloatSignal('f2',uc[1]) 171 | sim.setFloatSignal('f3',uc[2]) 172 | sim.setFloatSignal('theta1',uc[3]) 173 | sim.setFloatSignal('theta2',uc[4]) 174 | sim.setFloatSignal('theta3',uc[5]) 175 | 176 | def run_omni_hexcoper(t): 177 | 178 | 179 | pd=np.array([np.cos(0.5*t),np.sin(0.5*t),np.cos(2*t)+1.5]) 180 | dpd=np.array([-0.5*np.sin(0.5*t),0.5*np.cos(0.5*t),-2*np.sin(2*t)]) 181 | ddpd=np.array([-0.25*np.cos(0.5*t),-0.25*np.sin(0.5*t),-4*np.cos(2*t)]) 182 | 183 | Rd=rot.euler_to_rot([np.sin(0.5*t),np.cos(0.5*t),0]) 184 | dRd=(rot.euler_to_rot([np.sin(0.5*(t+dt)),np.cos(0.5*(t+dt)),0]) 185 | -rot.euler_to_rot([np.sin(0.5*t),np.cos(0.5*t),0]))/dt 186 | 187 | omni_hex.set_target(pd,dpd,ddpd,Rd,dRd) 188 | 189 | 190 | #获取无人机状态 191 | p=np.array(sim.getObjectPosition(UAV,-1)) 192 | v,w=sim.getObjectVelocity(UAV) 193 | v=np.array(v) 194 | w=np.array(w) 195 | R=np.array(sim.getObjectMatrix(UAV,-1)).reshape(3,4)[0:3,0:3] 196 | 197 | omni_hex.set_state(p,v,R,w) 198 | 199 | uc=omni_hex.run() 200 | 201 | sim.setFloatSignal('f1',uc[0]) 202 | sim.setFloatSignal('f2',uc[1]) 203 | sim.setFloatSignal('f3',uc[2]) 204 | sim.setFloatSignal('f4',uc[3]) 205 | sim.setFloatSignal('f5',uc[4]) 206 | sim.setFloatSignal('f6',uc[5]) 207 | 208 | 209 | try: 210 | print('Simulation started') 211 | client.setStepping(True) 212 | 213 | sim.startSimulation() 214 | 215 | t=sim.getSimulationTime() 216 | 217 | while True: 218 | t=sim.getSimulationTime() 219 | 220 | # 取消注释选择需要控制的无人机 221 | #run_quadcoper(t) #四旋翼 222 | #run_tricoper(t) #倾转三旋翼 223 | #run_bicoper(t) #倾转双旋翼 224 | run_singlecoper(t) #共轴双旋翼 225 | #run_omni_tricoper(t) #全驱动三旋翼 226 | #run_omni_hexcoper(t) #全驱动六旋翼 227 | 228 | client.step() 229 | #print(sim.getSimulationTime()-t) 230 | 231 | except KeyboardInterrupt: 232 | pass 233 | 234 | sim.stopSimulation() 235 | print('Simulation stopped') -------------------------------------------------------------------------------- /attitude_control/python/main.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import rotation as rot 3 | import time 4 | try: 5 | from zmqRemoteApi import RemoteAPIClient 6 | except: 7 | print("Can not find zmqRemoteApi lib") 8 | 9 | 10 | client = RemoteAPIClient() 11 | sim = client.getObject('sim') 12 | 13 | body_euler=sim.getObject('/body_euler') 14 | target_euler=sim.getObject('/target_euler') 15 | 16 | body_quat=sim.getObject('/body_quat') 17 | target_quat=sim.getObject('/target_quat') 18 | 19 | body_so3=sim.getObject('/body_so3') 20 | target_so3=sim.getObject('/target_so3') 21 | 22 | t=0 23 | dt=0.02 24 | Rd=np.identity(3) 25 | last_Rd=np.identity(3) 26 | wd=np.array([0,0,0],dtype=float) 27 | last_wd=np.array([0,0,0],dtype=float) 28 | dwd=np.array([0,0,0,],dtype=float) 29 | 30 | J=0.01*np.identity(3) #转动惯量 31 | 32 | def euler_ctrl_kine(): 33 | ''' 34 | 从仿真得到的一维数组结构为： 35 | 36 | R t 37 | 0, 1, 2, 3 38 | 4, 5, 6, 7 39 | 8, 9, 10, 11 40 | ''' 41 | Rb=np.array(sim.getObjectMatrix(body_euler,-1)).reshape(3,4)[0:3,0:3] 42 | 43 | Phi_d=rot.rot_to_euler(Rd)[0] 44 | Phi_b=rot.rot_to_euler(Rb)[0] 45 | 46 | Phi_e=Phi_b-Phi_d 47 | T_inv=np.array([[1,0,-np.sin(Phi_b[1])], 48 | [0,np.cos(Phi_b[0]),np.sin(Phi_b[0])*np.cos(Phi_b[1])], 49 | [0,-np.sin(Phi_b[0]),np.cos(Phi_b[0])*np.cos(Phi_b[1])]]) 50 | K=1 51 | w=-K*T_inv@Phi_e 52 | 53 | sim.setFloatSignal('ux_euler',w[0]) 54 | sim.setFloatSignal('uy_euler',w[1]) 55 | sim.setFloatSignal('uz_euler',w[2]) 56 | 57 | def quat_ctrl_kine(): 58 | qd=rot.rot_to_quat(Rd) 59 | invqd=np.array([qd[0],-qd[1],-qd[2],-qd[3]]) 60 | q=sim.getObjectQuaternion(body_quat,-1) 61 | q=np.array([q[3],q[0],q[1],q[2]]) 62 | 63 | 64 | qe=rot.qtimes(invqd,q) 65 | 66 | K=2 67 | w=-np.sign(qe[0])*K*qe[1:] 68 | 69 | sim.setFloatSignal('ux_quat',w[0]) 70 | sim.setFloatSignal('uy_quat',w[1]) 71 | sim.setFloatSignal('uz_quat',w[2]) 72 | 73 | 74 | def so3_ctrl_kine(): 75 | Rb=np.array(sim.getObjectMatrix(body_so3,-1)).reshape(3,4)[0:3,0:3] 76 | Re=Rd.T@Rb 77 | 78 | K=1 79 | w=-K*rot.so3_vee(Re-Re.T) 80 | 81 | sim.setFloatSignal('ux_so3',w[0]) 82 | sim.setFloatSignal('uy_so3',w[1]) 83 | sim.setFloatSignal('uz_so3',w[2]) 84 | 85 | 86 | def euler_ctrl_dyna(): 87 | 88 | Phi_d=rot.rot_to_euler(Rd)[0] 89 | 90 | sxd=np.sin(Phi_d[0]) 91 | cxd=np.cos(Phi_d[0]) 92 | 93 | syd=np.sin(Phi_d[1]) 94 | cyd=np.cos(Phi_d[1]) 95 | tyd=np.tan(Phi_d[1]) 96 | if cyd != 0: 97 | secyd=1/cyd 98 | else: 99 | secyd=1e8 100 | 101 | Td=np.array([[1,sxd*tyd,cxd*tyd], 102 | [0,cxd,-syd], 103 | [0,-sxd*secyd,cxd*secyd]]) 104 | 105 | dPhi_d=Td@wd 106 | ddPhi_d=Td@dwd 107 | 108 | 109 | Rb=np.array(sim.getObjectMatrix(body_euler,-1)).reshape(3,4)[0:3,0:3] 110 | Phi_b=rot.rot_to_euler(Rb)[0] 111 | 112 | Phi_e=Phi_b-Phi_d 113 | sx=np.sin(Phi_b[0]) 114 | cx=np.cos(Phi_b[0]) 115 | 116 | sy=np.sin(Phi_b[1]) 117 | cy=np.cos(Phi_b[1]) 118 | ty=np.tan(Phi_b[1]) 119 | if cy != 0: 120 | secy=1/cy 121 | else: 122 | secy=1e8 123 | 124 | T=np.array([[1,sx*ty,cx*ty], 125 | [0,cx,-sy], 126 | [0,-sx*secy,cx*secy]]) 127 | 128 | T_inv=np.array([[1,0,-sy], 129 | [0,cx,sx*cy], 130 | [0,-sx,cx*cy]]) 131 | wb=np.array(sim.getObjectVelocity(body_euler)[1]) 132 | A=np.array([[cx*ty,sx*secy*secy,0], 133 | [-sx,0,0], 134 | [cx*secy,sx*secy*ty,0]]) 135 | B=np.array([[-sx*ty,cx*secy*secy,0], 136 | [-cx,0,0], 137 | [-sx*secy,cx*secy*ty,0]]) 138 | dT=np.c_[np.array([0,0,0]),np.c_[A@T@wb,B@T@wb]] 139 | 140 | 141 | K1=2 142 | K2=2 143 | 144 | Phi_z=T@wb+K1*Phi_e-dPhi_d 145 | tau=rot.sk(wb)@J@wb-J@T_inv@(K2*Phi_z+dT@wb \ 146 | +K1*T@wb+Phi_e-K1*dPhi_d-ddPhi_d) 147 | 148 | sim.setFloatSignal('ux_euler',tau[0]) 149 | sim.setFloatSignal('uy_euler',tau[1]) 150 | sim.setFloatSignal('uz_euler',tau[2]) 151 | 152 | def quat_ctrl_dyna(): 153 | 154 | qd=rot.rot_to_quat(Rd) 155 | invqd=np.array([qd[0],-qd[1],-qd[2],-qd[3]]) 156 | 157 | q=sim.getObjectQuaternion(body_quat,-1) 158 | q=np.array([q[3],q[0],q[1],q[2]]) 159 | wb=np.array(sim.getObjectVelocity(body_quat)[1]) 160 | 161 | qe=rot.qtimes(invqd,q) 162 | qe=np.sign(qe[0])*qe 163 | se=qe[0] 164 | ve=qe[1:] 165 | 166 | dve=0.5*(se*(wb-wd)+rot.sk(ve)@(wb+wd)) 167 | 168 | K1=2 169 | K2=2 170 | 171 | vz=wb+2*K1*ve-wd 172 | tau=rot.sk(wb)@J@wb+J@(-K2*vz-2*K1*dve-0.5*ve+dwd) 173 | 174 | sim.setFloatSignal('ux_quat',tau[0]) 175 | sim.setFloatSignal('uy_quat',tau[1]) 176 | sim.setFloatSignal('uz_quat',tau[2]) 177 | 178 | def so3_ctrl_dyna(): 179 | 180 | #当前姿态、角速度 181 | Rb=np.array(sim.getObjectMatrix(body_so3,-1)).reshape(3,4)[0:3,0:3] 182 | wb=np.array(sim.getObjectVelocity(body_so3)[1]) 183 | 184 | #误差 185 | Re=Rd.T@Rb 186 | phi_e=rot.so3_vee(Re-Re.T) 187 | dRe=Re@rot.sk(wb)-rot.sk(wd)@Re 188 | dphi_e=rot.so3_vee(dRe-dRe.T) 189 | Omega_e=dRe.T@wd+Re.T@dwd 190 | 191 | K1=2 192 | K2=2 193 | J=0.01*np.identity(3) 194 | 195 | z=wb+K1*phi_e-Re.T@wd 196 | tau=rot.sk(wb)@J@wb+J@(-K2*z-phi_e-K1*dphi_e+Omega_e) 197 | 198 | sim.setFloatSignal('ux_so3',tau[0]) 199 | sim.setFloatSignal('uy_so3',tau[1]) 200 | sim.setFloatSignal('uz_so3',tau[2]) 201 | 202 | 203 | try: 204 | print('Simulation started') 205 | client.setStepping(True) 206 | sim.startSimulation() 207 | 208 | t=sim.getSimulationTime() 209 | 210 | Rd=rot.euler_to_rot([0.5*np.cos(2*t),2*np.cos(2*t),0.5*np.sin(2*t)]) 211 | #Rd=rot.quat_to_rot([-1,0,0,0]) 212 | last_Rd=Rd.copy() 213 | 214 | #仿真环境中的欧拉角是xyz顺序的，因此转为四元数再发送 215 | #仿真中的四元数是xyzw顺序 216 | qd=rot.rot_to_quat(Rd) 217 | sim.setObjectQuaternion(target_euler,-1,[qd[1],qd[2],qd[3],qd[0]]) 218 | sim.setObjectQuaternion(target_quat,-1,[qd[1],qd[2],qd[3],qd[0]]) 219 | sim.setObjectQuaternion(target_so3,-1,[qd[1],qd[2],qd[3],qd[0]]) 220 | 221 | print("- Input 0 for kinematics controller ") 222 | print("- Input 1 for dynamics tracking controller ") 223 | 224 | ctrl_mode=1 225 | try: 226 | ctrl_mode=int(input()) 227 | if ctrl_mode: 228 | print("Start with dynamics controller") 229 | else: 230 | print("Start with kinematics controller") 231 | except: 232 | print("Defaut start with dynamics controller") 233 | 234 | 235 | while True: 236 | t=sim.getSimulationTime() 237 | 238 | sim.setInt32Signal('ctrl_mode',ctrl_mode) 239 | #生成一段旋转轨迹 240 | Rd=rot.euler_to_rot([0.5*np.cos(2*t),2*np.cos(2*t),0.5*np.sin(2*t)]) 241 | 242 | dRd=(Rd-last_Rd)/dt 243 | 244 | wd=rot.so3_vee(Rd.T@dRd) 245 | dwd=(wd-last_wd)/dt 246 | 247 | last_wd=wd.copy() 248 | last_Rd=Rd.copy() 249 | 250 | qd=rot.rot_to_quat(Rd) 251 | sim.setObjectQuaternion(target_euler,-1,[qd[1],qd[2],qd[3],qd[0]]) 252 | sim.setObjectQuaternion(target_quat,-1,[qd[1],qd[2],qd[3],qd[0]]) 253 | sim.setObjectQuaternion(target_so3,-1,[qd[1],qd[2],qd[3],qd[0]]) 254 | 255 | if ctrl_mode: 256 | #动力学控制 257 | euler_ctrl_dyna() 258 | quat_ctrl_dyna() 259 | so3_ctrl_dyna() 260 | else: 261 | #运动学控制 262 | euler_ctrl_kine() 263 | quat_ctrl_kine() 264 | so3_ctrl_kine() 265 | 266 | client.step() 267 | #print(sim.getSimulationTime()-t) 268 | 269 | except KeyboardInterrupt: 270 | pass 271 | 272 | sim.stopSimulation() 273 | print('Simulation stopped') -------------------------------------------------------------------------------- /multicopter_control/python/uav_controller.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from base_controller import* 3 | import rotation as rot 4 | 5 | class quadcopter(): 6 | def __init__(self,dt): 7 | self.dt=dt #控制步长 8 | self.posi_ctrl=posi_ctrl(dt) 9 | self.atti_ctrl=atti_ctrl(dt) 10 | 11 | self.k=0.005 #旋翼的力矩升力比 12 | self.l=0.1 #机臂参数 13 | #混控矩阵 14 | self.mix=np.array([[self.l*self.k,self.k,-self.k,self.l], 15 | [self.l*self.k,self.k,self.k,-self.l], 16 | [self.l*self.k,-self.k,self.k,self.l], 17 | [self.l*self.k,-self.k,-self.k,-self.l]])/(4*self.l*self.k) 18 | #期望偏航 19 | self.yawd=0.0 20 | 21 | pass 22 | 23 | def set_target(self,pd,dpd,ddpd,yawd): 24 | self.posi_ctrl.pd=pd 25 | self.posi_ctrl.dpd=dpd 26 | self.posi_ctrl.ddpd=ddpd 27 | self.yawd=yawd 28 | 29 | def set_state(self,p,v,R,w): 30 | self.posi_ctrl.p=p 31 | self.posi_ctrl.v=v 32 | self.atti_ctrl.R=R 33 | self.atti_ctrl.w=w 34 | 35 | def run(self): 36 | 37 | #计算位置控制器 38 | f=self.posi_ctrl.compute() 39 | 40 | #由位置控制器的输入计算期望姿态矩阵 41 | h=np.array([np.cos(self.yawd),np.sin(self.yawd),0]) 42 | fz=np.linalg.norm(f) 43 | n3=f/fz 44 | n2=np.cross(n3,h) 45 | n2=n2/np.linalg.norm(n2) 46 | n1=np.cross(n2,n3) 47 | 48 | Rd=np.c_[n1,n2,n3] 49 | self.atti_ctrl.dRd=(Rd-self.atti_ctrl.Rd)/self.dt 50 | self.atti_ctrl.Rd=Rd 51 | 52 | tau=self.atti_ctrl.compute() 53 | u=np.array([fz,tau[0],tau[1],tau[2]]) 54 | 55 | fi=self.mix@u 56 | 57 | return fi 58 | 59 | 60 | class tricopter(): 61 | def __init__(self,dt): 62 | self.dt=dt #控制步长 63 | self.posi_ctrl=posi_ctrl(dt) 64 | self.atti_ctrl=atti_ctrl(dt) 65 | 66 | self.k=0.005 #旋翼的力矩升力比 67 | self.l=0.2 #机臂参数 68 | 69 | #混控矩阵 70 | self.mix=np.array([[self.l**2+self.k**2,np.sqrt(3)*self.l,-self.l,-self.k], 71 | [self.l**2+self.k**2,-np.sqrt(3)*self.l,-self.l,-self.k], 72 | [self.l**2-2*self.k**2,0,2*self.l,2*self.k], 73 | [-3*self.l*self.k,0,0,3*self.l]])/(3*self.l**2) 74 | 75 | #期望偏航 76 | self.yawd=0.0 77 | 78 | #调节控制增益 79 | self.posi_ctrl.kp=np.diag([3,3,3]) 80 | self.posi_ctrl.kd=np.diag([3,3,3]) 81 | self.posi_ctrl.ki=np.diag([0.1,0.1,0.1]) 82 | 83 | self.atti_ctrl.k1=np.diag([0.5,0.5,0.01]) 84 | self.atti_ctrl.k2=np.diag([8,8,2]) 85 | self.atti_ctrl.ki=np.diag([0.01,0.01,0.001]) 86 | 87 | def set_target(self,pd,dpd,ddpd,yawd): 88 | self.posi_ctrl.pd=pd 89 | self.posi_ctrl.dpd=dpd 90 | self.posi_ctrl.ddpd=ddpd 91 | self.yawd=yawd 92 | 93 | def set_state(self,p,v,R,w): 94 | self.posi_ctrl.p=p 95 | self.posi_ctrl.v=v 96 | self.atti_ctrl.R=R 97 | self.atti_ctrl.w=w 98 | 99 | def run(self): 100 | 101 | #计算位置控制器 102 | f=self.posi_ctrl.compute() 103 | 104 | 105 | #由位置控制器的输入计算期望姿态矩阵 106 | h=np.array([np.cos(self.yawd),np.sin(self.yawd),0]) 107 | fz=np.linalg.norm(f) 108 | n3=f/fz 109 | n2=np.cross(n3,h) 110 | n2=n2/np.linalg.norm(n2) 111 | n1=np.cross(n2,n3) 112 | 113 | Rd=np.c_[n1,n2,n3] 114 | self.atti_ctrl.dRd=(Rd-self.atti_ctrl.Rd)/self.dt 115 | self.atti_ctrl.Rd=Rd 116 | 117 | tau=self.atti_ctrl.compute() 118 | u=np.array([fz,tau[0],tau[1],tau[2]]) 119 | 120 | 121 | fi=self.mix@u 122 | 123 | f3=np.sqrt(fi[2]**2+fi[3]**2) 124 | theta=np.arctan2(fi[3],fi[2]) 125 | 126 | return np.array([fi[0],fi[1],f3,theta]) 127 | 128 | 129 | class bicopter(): 130 | def __init__(self,dt): 131 | self.dt=dt #控制步长 132 | self.posi_ctrl=posi_ctrl(dt) 133 | self.atti_ctrl=atti_ctrl(dt) 134 | 135 | self.k=0.00 #旋翼的力矩升力比 136 | self.ly=0.15 #机臂参数 137 | self.lz=0.15 138 | 139 | #混控矩阵 140 | x=self.ly**2+self.k**2 141 | self.mix=0.5*np.array([[1, self.ly/x, 0, self.k/x], 142 | [0, self.k/x, 1/self.lz, -self.ly/x], 143 | [1, -self.ly/x, 0, -self.k/x], 144 | [0, -self.k/x, 1/self.lz, self.ly/x]]) 145 | 146 | #期望偏航 147 | self.yawd=0.0 148 | 149 | #调节控制增益 150 | self.posi_ctrl.kp=np.diag([2,2,3]) 151 | self.posi_ctrl.kd=np.diag([2,2,3]) 152 | self.posi_ctrl.ki=np.diag([0.,0.0,0.]) 153 | 154 | self.atti_ctrl.k1=np.diag([0.5,0.05,0.005]) 155 | self.atti_ctrl.k2=np.diag([8,3,2]) 156 | self.atti_ctrl.ki=np.diag([0.00,0.00,0.00]) 157 | 158 | def set_target(self,pd,dpd,ddpd,yawd): 159 | self.posi_ctrl.pd=pd 160 | self.posi_ctrl.dpd=dpd 161 | self.posi_ctrl.ddpd=ddpd 162 | self.yawd=yawd 163 | 164 | def set_state(self,p,v,R,w): 165 | self.posi_ctrl.p=p 166 | self.posi_ctrl.v=v 167 | self.atti_ctrl.R=R 168 | self.atti_ctrl.w=w 169 | 170 | def run(self): 171 | 172 | #计算位置控制器 173 | f=self.posi_ctrl.compute() 174 | 175 | #由位置控制器的输入计算期望姿态矩阵 176 | h=np.array([np.cos(self.yawd),np.sin(self.yawd),0]) 177 | fz=np.linalg.norm(f) 178 | n3=f/fz 179 | n2=np.cross(n3,h) 180 | n2=n2/np.linalg.norm(n2) 181 | n1=np.cross(n2,n3) 182 | 183 | Rd=np.c_[n1,n2,n3] 184 | self.atti_ctrl.dRd=(Rd-self.atti_ctrl.Rd)/self.dt 185 | self.atti_ctrl.Rd=Rd 186 | 187 | tau=self.atti_ctrl.compute() 188 | u=np.array([fz,tau[0],tau[1],tau[2]]) 189 | 190 | 191 | fi=self.mix@u 192 | 193 | f1=np.sqrt(fi[0]**2+fi[1]**2) 194 | theta1=np.arctan2(fi[1],fi[0]) 195 | f2=np.sqrt(fi[2]**2+fi[3]**2) 196 | theta2=np.arctan2(fi[3],fi[2]) 197 | 198 | return np.array([f1,f2,theta1,theta2]) 199 | 200 | class singlecopter(): 201 | def __init__(self,dt): 202 | self.dt=dt #控制步长 203 | self.posi_ctrl=posi_ctrl(dt) 204 | self.atti_ctrl=atti_ctrl(dt) 205 | 206 | self.k=0.005 #旋翼的力矩升力比 207 | self.l=0.1 #机臂参数 208 | 209 | #混控矩阵 210 | x=self.l**2+self.k**2 211 | self.mix=np.array([[0.5, 0, 0, 0.5/self.k], 212 | [0, self.l/x, -self.k/x, 0], 213 | [0.5, 0, 0, -0.5/self.k], 214 | [0, -self.k/x, -self.l/x, 0]]) 215 | 216 | #期望偏航 217 | self.yawd=0 218 | 219 | #调节控制增益 220 | self.posi_ctrl.kp=np.diag([0.5,0.5,3]) 221 | self.posi_ctrl.kd=np.diag([1,1,3]) 222 | self.posi_ctrl.ki=np.diag([0.00,0.00,0.5]) 223 | 224 | self.atti_ctrl.k1=np.diag([0.1,0.1,0.005]) 225 | self.atti_ctrl.k2=np.diag([2,2,0.6]) 226 | self.atti_ctrl.ki=np.diag([0.00,0.00,0.0]) 227 | 228 | def set_target(self,pd,dpd,ddpd,yawd): 229 | self.posi_ctrl.pd=pd 230 | self.posi_ctrl.dpd=dpd 231 | self.posi_ctrl.ddpd=ddpd 232 | self.yawd=yawd 233 | 234 | def set_state(self,p,v,R,w): 235 | self.posi_ctrl.p=p 236 | self.posi_ctrl.v=v 237 | self.atti_ctrl.R=R 238 | self.atti_ctrl.w=w 239 | 240 | def run(self): 241 | 242 | #计算位置控制器 243 | f=self.posi_ctrl.compute() 244 | 245 | #由位置控制器的输入计算期望姿态矩阵 246 | h=np.array([np.cos(self.yawd),np.sin(self.yawd),0]) 247 | fz=np.linalg.norm(f) 248 | n3=f/fz 249 | n2=np.cross(n3,h) 250 | n2=n2/np.linalg.norm(n2) 251 | n1=np.cross(n2,n3) 252 | 253 | Rd=np.c_[n1,n2,n3] 254 | self.atti_ctrl.dRd=(Rd-self.atti_ctrl.Rd)/self.dt 255 | self.atti_ctrl.Rd=Rd 256 | 257 | tau=self.atti_ctrl.compute() 258 | u=np.array([fz,tau[0],tau[1],tau[2]]) 259 | 260 | 261 | fi=self.mix@u 262 | 263 | f1=np.sqrt(fi[0]**2+fi[1]**2) 264 | theta1=np.arctan2(fi[1],fi[0]) 265 | f2=np.sqrt(fi[2]**2+fi[3]**2) 266 | theta2=np.arctan2(fi[3],fi[2]) 267 | 268 | 269 | return np.array([f1,f2,theta1,theta2]) 270 | 271 | 272 | class omni_tricopter(): 273 | def __init__(self,dt): 274 | self.dt=dt #控制步长 275 | self.posi_ctrl=posi_ctrl(dt) 276 | self.atti_ctrl=atti_ctrl(dt) 277 | 278 | self.k=0.005 #旋翼的力矩升力比 279 | self.l=0.2 #机臂参数 280 | 281 | #注意：由于这里的混控关系形式比较复杂，这里给出的矩阵是求逆之前的混控矩阵 282 | self.pmix=np.array([[0,-0.5*np.sqrt(3),0,-0.5*np.sqrt(3),0,0], 283 | [0,0.5,0,-0.5,0,-1], 284 | [1, 0, 1, 0, 1, 0], 285 | [0.5*np.sqrt(3)*self.l, -0.5*np.sqrt(3)*self.k, -0.5*np.sqrt(3)*self.l, 0.5*np.sqrt(3)*self.k, 0, 0], 286 | [-0.5*self.l, 0.5*self.k, -0.5*self.l, 0.5*self.k, self.l, -self.k], 287 | [self.k, self.l, -self.k, -self.l, self.k, self.l]],dtype=float) 288 | 289 | 290 | #调节控制增益 291 | self.posi_ctrl.kp=np.diag([1,1,1]) 292 | self.posi_ctrl.kd=np.diag([1.5,1.5,1.5]) 293 | self.posi_ctrl.ki=np.diag([0.0,0.0,0.1]) 294 | 295 | self.atti_ctrl.k1=np.diag([0.2,0.2,0.01]) 296 | self.atti_ctrl.k2=np.diag([12,12,2]) 297 | self.atti_ctrl.ki=np.diag([0.0,0.0,0.0]) 298 | 299 | def set_target(self,pd,dpd,ddpd, Rd, dRd): 300 | self.posi_ctrl.pd=pd 301 | self.posi_ctrl.dpd=dpd 302 | self.posi_ctrl.ddpd=ddpd 303 | self.atti_ctrl.Rd=Rd 304 | self.atti_ctrl.dRd=dRd 305 | 306 | def set_state(self,p,v,R,w): 307 | self.posi_ctrl.p=p 308 | self.posi_ctrl.v=v 309 | self.atti_ctrl.R=R 310 | self.atti_ctrl.w=w 311 | 312 | def run(self): 313 | 314 | #计算位置控制器 315 | f=self.atti_ctrl.R.T@self.posi_ctrl.compute() 316 | tau=self.atti_ctrl.compute() 317 | u=np.array([f[0],f[1],f[2],tau[0],tau[1],tau[2]]) 318 | 319 | 320 | fi=np.linalg.inv(self.pmix)@u 321 | 322 | f1=np.sqrt(fi[0]**2+fi[1]**2) 323 | theta1=np.arctan2(fi[1],fi[0]) 324 | f2=np.sqrt(fi[2]**2+fi[3]**2) 325 | theta2=np.arctan2(fi[3],fi[2]) 326 | f3=np.sqrt(fi[4]**2+fi[5]**2) 327 | theta3=np.arctan2(fi[5],fi[4]) 328 | 329 | 330 | return np.array([f1,f2,f3,theta1,theta2,theta3]) 331 | 332 | 333 | 334 | class omni_hexcopter(): 335 | def __init__(self,dt): 336 | self.dt=dt #控制步长 337 | self.posi_ctrl=posi_ctrl(dt) 338 | self.atti_ctrl=atti_ctrl(dt) 339 | 340 | self.k=0.005 #旋翼的力矩升力比 341 | self.l=0.2 #机臂参数 342 | lpk=self.l+self.k 343 | lnk=self.l-self.k 344 | #混控矩阵 345 | self.mix=np.array([[lnk,-np.sqrt(3)*lnk,lpk,1,-np.sqrt(3),-1], 346 | [lpk,np.sqrt(3)*lpk,lnk,-1,-np.sqrt(3),1], 347 | [-2*lpk, 0, lnk, 2, 0, 1], 348 | [-2*lnk, 0, lpk, -2, 0, -1], 349 | [lnk,np.sqrt(3)*lnk,lpk,1,np.sqrt(3),-1], 350 | [lpk,-np.sqrt(3)*lpk,lnk,-1,np.sqrt(3),1]],dtype=float)*(np.sqrt(2)/(6*self.l)) 351 | 352 | 353 | #调节控制增益 354 | self.posi_ctrl.kp=np.diag([6,6,6]) 355 | self.posi_ctrl.kd=np.diag([3,3,3]) 356 | self.posi_ctrl.ki=np.diag([1,1,1]) 357 | 358 | self.atti_ctrl.k1=np.diag([0.5,0.5,0.5]) 359 | self.atti_ctrl.k2=np.diag([12,12,12]) 360 | self.atti_ctrl.ki=np.diag([0.0,0.0,0.0]) 361 | 362 | def set_target(self,pd,dpd,ddpd, Rd, dRd): 363 | self.posi_ctrl.pd=pd 364 | self.posi_ctrl.dpd=dpd 365 | self.posi_ctrl.ddpd=ddpd 366 | self.atti_ctrl.Rd=Rd 367 | self.atti_ctrl.dRd=dRd 368 | 369 | def set_state(self,p,v,R,w): 370 | self.posi_ctrl.p=p 371 | self.posi_ctrl.v=v 372 | self.atti_ctrl.R=R 373 | self.atti_ctrl.w=w 374 | 375 | def run(self): 376 | 377 | #计算位置控制器 378 | f=self.atti_ctrl.R.T@self.posi_ctrl.compute() 379 | tau=self.atti_ctrl.compute() 380 | u=np.array([f[0],f[1],f[2],tau[0],tau[1],tau[2]]) 381 | 382 | 383 | fi=self.mix@u 384 | 385 | return np.array([fi[0],fi[1],fi[2],fi[3],fi[4],fi[5]]) -------------------------------------------------------------------------------- /attitude_control/python/zmqRemoteApi/__init__.py: -------------------------------------------------------------------------------- 1 | """CoppeliaSim's Remote API client.""" 2 | 3 | import os 4 | 5 | import uuid 6 | 7 | from time import sleep 8 | 9 | import cbor 10 | 11 | import zmq 12 | 13 | import math 14 | 15 | def b64(b): 16 | import base64 17 | return base64.b64encode(b).decode('ascii') 18 | 19 | 20 | class RemoteAPIClient: 21 | """Client to connect to CoppeliaSim's ZMQ Remote API.""" 22 | 23 | def __init__(self, host='localhost', port=23000, cntport=None, *, verbose=None): 24 | """Create client and connect to the ZMQ Remote API server.""" 25 | self.verbose = int(os.environ.get('VERBOSE', '0')) if verbose is None else verbose 26 | self.context = zmq.Context() 27 | self.socket = self.context.socket(zmq.REQ) 28 | self.cntsocket = self.context.socket(zmq.SUB) 29 | self.socket.connect(f'tcp://{host}:{port}') 30 | self.cntsocket.setsockopt(zmq.SUBSCRIBE, b'') 31 | self.cntsocket.setsockopt(zmq.CONFLATE, 1) 32 | self.cntsocket.connect(f'tcp://{host}:{cntport if cntport else port+1}') 33 | self.uuid = str(uuid.uuid4()) 34 | self.threadLocLevel = 0 35 | 36 | def __del__(self): 37 | """Disconnect and destroy client.""" 38 | self.socket.close() 39 | self.cntsocket.close() 40 | self.context.term() 41 | 42 | def _send(self, req): 43 | if self.verbose > 0: 44 | print('Sending:', req) 45 | rawReq = cbor.dumps(req) 46 | if self.verbose > 1: 47 | print(f'Sending raw len={len(rawReq)}, base64={b64(rawReq)}') 48 | self.socket.send(rawReq) 49 | 50 | def _recv(self): 51 | rawResp = self.socket.recv() 52 | if self.verbose > 1: 53 | print(f'Received raw len={len(rawResp)}, base64={b64(rawResp)}') 54 | resp = cbor.loads(rawResp) 55 | if self.verbose > 0: 56 | print('Received:', resp) 57 | return resp 58 | 59 | def _process_response(self, resp): 60 | if not resp.get('success', False): 61 | raise Exception(resp.get('error')) 62 | ret = resp['ret'] 63 | if len(ret) == 1: 64 | return ret[0] 65 | if len(ret) > 1: 66 | return tuple(ret) 67 | 68 | def call(self, func, args): 69 | """Call function with specified arguments.""" 70 | self._send({'func': func, 'args': args}) 71 | return self._process_response(self._recv()) 72 | 73 | def getObject(self, name, _info=None): 74 | """Retrieve remote object from server.""" 75 | ret = type(name, (), {}) 76 | if not _info: 77 | _info = self.call('zmqRemoteApi.info', [name]) 78 | for k, v in _info.items(): 79 | if not isinstance(v, dict): 80 | raise ValueError('found nondict') 81 | if len(v) == 1 and 'func' in v: 82 | setattr(ret, k, lambda *a, func=f'{name}.{k}': self.call(func, a)) 83 | elif len(v) == 1 and 'const' in v: 84 | setattr(ret, k, v['const']) 85 | else: 86 | setattr(ret, k, self.getObject(f'{name}.{k}', _info=v)) 87 | if name=="sim": 88 | ret.wait=self._wait 89 | ret.waitForSignal=self._waitForSignal 90 | ret.moveToConfig=self._moveToConfig 91 | ret.moveToPose=self._moveToPose 92 | self.sim=ret 93 | return ret 94 | 95 | def setStepping(self, enable=True): 96 | if self.threadLocLevel > 0: 97 | if enable == False: 98 | self.threadLocLevel = 0 99 | return self.call('setStepping', [enable,self.uuid]) 100 | else: 101 | if enable == True: 102 | self.threadLocLevel = 1 103 | return self.call('setStepping', [enable,self.uuid]) 104 | 105 | def step(self, *, wait=True): 106 | if self.threadLocLevel > 0: 107 | self.getStepCount(False) 108 | self.call('step', [self.uuid]) 109 | self.getStepCount(wait) 110 | 111 | def getStepCount(self, wait): 112 | if self.threadLocLevel > 0: 113 | try: 114 | self.cntsocket.recv(0 if wait else zmq.NOBLOCK) 115 | except zmq.ZMQError: 116 | pass 117 | 118 | def _setThreadAutomaticSwitch(self, level): 119 | newLevel = self.threadLocLevel 120 | if isinstance(level,bool): 121 | if level == True: 122 | newLevel -= 1 123 | if newLevel < 0: 124 | newLevel = 0 125 | if level == False: 126 | newLevel += 1 127 | else: 128 | if level >= 0: 129 | newLevel = level 130 | if newLevel != self.threadLocLevel: 131 | if newLevel == 0: 132 | self.setStepping(False) 133 | if newLevel == 1 and self.threadLocLevel == 0: 134 | self.setStepping(True) 135 | self.threadLocLevel = newLevel 136 | return newLevel 137 | 138 | def _wait(self, dt, simTime=True): 139 | lb=self._setThreadAutomaticSwitch(False) 140 | retVal = 0.0 141 | if simTime: 142 | st = self.sim.getSimulationTime() 143 | while (self.sim.getSimulationTime()-st < dt): 144 | self.step() 145 | retVal=self.sim.getSimulationTime()-st-dt 146 | else: 147 | st = self.sim.getSystemTimeInMs(-1) 148 | while (self.sim.getSystemTimeInMs(st) < dt*1000): 149 | self.step() 150 | self._setThreadAutomaticSwitch(lb) 151 | return retVal 152 | 153 | def _waitForSignal(self, sigName): 154 | lb=self._setThreadAutomaticSwitch(False) 155 | retVal = 0.0 156 | while True: 157 | retVal = self.sim.getInt32Signal(sigName)!=None or self.sim.getFloatSignal(sigName)!=None or self.sim.getDoubleSignal(sigName)!=None or self.sim.getStringSignal(sigName)!=None 158 | if retVal: 159 | break 160 | self.step() 161 | self._setThreadAutomaticSwitch(lb) 162 | return retVal 163 | 164 | def _moveToConfig(self, flags,currentPos,currentVel,currentAccel,maxVel,maxAccel,maxJerk,targetPos,targetVel,callback,auxData=None,cyclicJoints=None,timeStep=0): 165 | lb=self._setThreadAutomaticSwitch(False) 166 | 167 | currentPosVelAccel=[] 168 | maxVelAccelJerk=[] 169 | targetPosVel=[] 170 | sel=[] 171 | outPos=[] 172 | outVel=[] 173 | outAccel=[] 174 | for i in range(len(currentPos)): 175 | v=currentPos[i] 176 | currentPosVelAccel.append(v) 177 | outPos.append(v) 178 | maxVelAccelJerk.append(maxVel[i]) 179 | w=targetPos[i] 180 | if cyclicJoints and cyclicJoints[i]: 181 | while w-v>=math.pi*2: 182 | w=w-math.pi*2 183 | while w-v<0: 184 | w=w+math.pi*2 185 | if w-v>math.pi: 186 | w=w-math.pi*2 187 | targetPosVel.append(w) 188 | sel.append(1) 189 | for i in range(len(currentPos)): 190 | if currentVel: 191 | currentPosVelAccel.append(currentVel[i]) 192 | outVel.append(currentVel[i]) 193 | else: 194 | currentPosVelAccel.append(0) 195 | outVel.append(0) 196 | maxVelAccelJerk.append(maxAccel[i]) 197 | if targetVel: 198 | targetPosVel.append(targetVel[i]) 199 | else: 200 | targetPosVel.append(0) 201 | for i in range(len(currentPos)): 202 | if currentAccel: 203 | currentPosVelAccel.append(currentAccel[i]) 204 | outAccel.append(currentAccel[i]) 205 | else: 206 | currentPosVelAccel.append(0) 207 | outAccel.append(0) 208 | maxVelAccelJerk.append(maxJerk[i]) 209 | 210 | if len(maxVel) > len(currentPos): 211 | for i in range(len(maxVel)-len(currentPos)): 212 | currentPosVelAccel.append(maxVel[len(currentPos)+i]) 213 | if len(maxAccel) > len(currentPos): 214 | for i in range(len(maxAccel)-len(currentPos)): 215 | currentPosVelAccel.append(maxAccel[len(currentPos)+i]) 216 | 217 | ruckigObject = self.sim.ruckigPos(len(currentPos),0.0001,flags,currentPosVelAccel,maxVelAccelJerk,sel,targetPosVel) 218 | result = 0 219 | timeLeft = 0 220 | while (result == 0): 221 | dt = timeStep 222 | if dt == 0: 223 | dt=self.sim.getSimulationTimeStep() 224 | syncTime = 0 225 | result,newPosVelAccel,syncTime = self.sim.ruckigStep(ruckigObject,dt) 226 | if result >= 0: 227 | if result == 0: 228 | timeLeft = dt-syncTime 229 | for i in range(len(currentPos)): 230 | outPos[i]=newPosVelAccel[i] 231 | outVel[i]=newPosVelAccel[len(currentPos)+i] 232 | outAccel[i]=newPosVelAccel[len(currentPos)*2+i] 233 | if callback(outPos,outVel,outAccel,auxData): 234 | break 235 | else: 236 | raise RuntimeError("sim.ruckigStep returned error code "+result) 237 | if result == 0: 238 | self.step() 239 | self.sim.ruckigRemove(ruckigObject) 240 | self._setThreadAutomaticSwitch(lb) 241 | return outPos,outVel,outAccel,timeLeft 242 | 243 | def _moveToPose(self, flags,currentPoseOrMatrix,maxVel,maxAccel,maxJerk,targetPoseOrMatrix,callback,auxData=None,metric=None,timeStep=0): 244 | lb = self._setThreadAutomaticSwitch(False) 245 | 246 | usingMatrices = (len(currentPoseOrMatrix)>=12) 247 | if usingMatrices: 248 | currentMatrix = currentPoseOrMatrix 249 | targetMatrix = targetPoseOrMatrix 250 | else: 251 | currentMatrix = self.sim.buildMatrixQ(currentPoseOrMatrix,[currentPoseOrMatrix[3],currentPoseOrMatrix[4],currentPoseOrMatrix[5],currentPoseOrMatrix[6]]) 252 | targetMatrix = self.sim.buildMatrixQ(targetPoseOrMatrix,[targetPoseOrMatrix[3],targetPoseOrMatrix[4],targetPoseOrMatrix[5],targetPoseOrMatrix[6]]) 253 | 254 | outMatrix = self.sim.copyTable(currentMatrix) 255 | axis,angle = self.sim.getRotationAxis(currentMatrix,targetMatrix) 256 | timeLeft = 0 257 | if metric: 258 | # Here we treat the movement as a 1 DoF movement, where we simply interpolate via t between 259 | # the start and goal pose. This always results in straight line movement paths 260 | dx = [(targetMatrix[3]-currentMatrix[3])*metric[0],(targetMatrix[7]-currentMatrix[7])*metric[1],(targetMatrix[11]-currentMatrix[11])*metric[2],angle*metric[3]] 261 | distance = math.sqrt(dx[0]*dx[0]+dx[1]*dx[1]+dx[2]*dx[2]+dx[3]*dx[3]) 262 | if distance > 0.000001: 263 | currentPosVelAccel = [0,0,0] 264 | maxVelAccelJerk = [maxVel[0],maxAccel[0],maxJerk[0]] 265 | if len(maxVel) > 1: 266 | maxVelAccelJerk.append(maxVel[1]) 267 | if len(maxAccel) > 1: 268 | maxVelAccelJerk.append(maxAccel[1]) 269 | targetPosVel = [distance,0] 270 | ruckigObject = self.sim.ruckigPos(1,0.0001,flags,currentPosVelAccel,maxVelAccelJerk,[1],targetPosVel) 271 | result = 0 272 | while (result == 0): 273 | dt = timeStep 274 | if dt == 0: 275 | dt = self.sim.getSimulationTimeStep() 276 | result,newPosVelAccel,syncTime = self.sim.ruckigStep(ruckigObject,dt) 277 | if result >= 0: 278 | if result == 0: 279 | timeLeft = dt-syncTime 280 | t = newPosVelAccel[0]/distance 281 | outMatrix = self.sim.interpolateMatrices(currentMatrix,targetMatrix,t) 282 | nv = [newPosVelAccel[1]] 283 | na = [newPosVelAccel[2]] 284 | if not usingMatrices: 285 | q = self.sim.getQuaternionFromMatrix(outMatrix) 286 | outMatrix = [outMatrix[3],outMatrix[7],outMatrix[11],q[0],q[1],q[2],q[3]] 287 | if callback(outMatrix,nv,na,auxData): 288 | break 289 | else: 290 | raise RuntimeError("sim.ruckigStep returned error code "+result) 291 | if result == 0: 292 | self.step() 293 | self.sim.ruckigRemove(ruckigObject) 294 | else: 295 | # Here we treat the movement as a 4 DoF movement, where each of X, Y, Z and rotation 296 | # is handled and controlled individually. This can result in non-straight line movement paths, 297 | # due to how the Ruckig functions operate depending on 'flags' 298 | dx = [targetMatrix[3]-currentMatrix[3],targetMatrix[7]-currentMatrix[7],targetMatrix[11]-currentMatrix[11],angle] 299 | currentPosVelAccel = [0,0,0,0,0,0,0,0,0,0,0,0] 300 | maxVelAccelJerk = [maxVel[0],maxVel[1],maxVel[2],maxVel[3],maxAccel[0],maxAccel[1],maxAccel[2],maxAccel[3],maxJerk[0],maxJerk[1],maxJerk[2],maxJerk[3]] 301 | if len(maxVel) > 4: 302 | for i in range(len(maxVel)-len(maxJerk)): 303 | maxVelAccelJerk.append(maxVel[len(maxJerk)+i]) 304 | if len(maxAccel) > 4: 305 | for i in range(len(maxAccel)-len(maxJerk)): 306 | maxVelAccelJerk.append(maxAccel[len(maxJerk)+i]) 307 | targetPosVel = [dx[0],dx[1],dx[2],dx[3],0,0,0,0,0] 308 | ruckigObject = self.sim.ruckigPos(4,0.0001,flags,currentPosVelAccel,maxVelAccelJerk,[1,1,1,1],targetPosVel) 309 | result = 0 310 | while (result == 0): 311 | dt = timeStep 312 | if dt == 0: 313 | dt = self.sim.getSimulationTimeStep() 314 | result,newPosVelAccel,syncTime = self.sim.ruckigStep(ruckigObject,dt) 315 | if result >= 0: 316 | if result == 0: 317 | timeLeft = dt-syncTime 318 | t = 0 319 | if abs(angle)>math.pi*0.00001: 320 | t = newPosVelAccel[3]/angle 321 | outMatrix = self.sim.interpolateMatrices(currentMatrix,targetMatrix,t) 322 | outMatrix[3] = currentMatrix[3]+newPosVelAccel[0] 323 | outMatrix[7] = currentMatrix[7]+newPosVelAccel[1] 324 | outMatrix[11] = currentMatrix[11]+newPosVelAccel[2] 325 | nv = [newPosVelAccel[4],newPosVelAccel[5],newPosVelAccel[6],newPosVelAccel[7]] 326 | na = [newPosVelAccel[8],newPosVelAccel[9],newPosVelAccel[10],newPosVelAccel[11]] 327 | if not usingMatrices: 328 | q = self.sim.getQuaternionFromMatrix(outMatrix) 329 | outMatrix = [outMatrix[3],outMatrix[7],outMatrix[11],q[0],q[1],q[2],q[3]] 330 | if callback(outMatrix,nv,na,auxData): 331 | break 332 | else: 333 | raise RuntimeError("sim.ruckigStep returned error code "+result) 334 | if result == 0: 335 | self.step() 336 | self.sim.ruckigRemove(ruckigObject) 337 | 338 | self._setThreadAutomaticSwitch(lb) 339 | return outMatrix,timeLeft 340 | 341 | 342 | if __name__ == '__console__': 343 | client = RemoteAPIClient() 344 | sim = client.getObject('sim') 345 | 346 | 347 | __all__ = ['RemoteAPIClient'] 348 | -------------------------------------------------------------------------------- /multicopter_control/python/zmqRemoteApi/__init__.py: -------------------------------------------------------------------------------- 1 | """CoppeliaSim's Remote API client.""" 2 | 3 | import os 4 | 5 | import uuid 6 | 7 | from time import sleep 8 | 9 | import cbor 10 | 11 | import zmq 12 | 13 | import math 14 | 15 | def b64(b): 16 | import base64 17 | return base64.b64encode(b).decode('ascii') 18 | 19 | 20 | class RemoteAPIClient: 21 | """Client to connect to CoppeliaSim's ZMQ Remote API.""" 22 | 23 | def __init__(self, host='localhost', port=23000, cntport=None, *, verbose=None): 24 | """Create client and connect to the ZMQ Remote API server.""" 25 | self.verbose = int(os.environ.get('VERBOSE', '0')) if verbose is None else verbose 26 | self.context = zmq.Context() 27 | self.socket = self.context.socket(zmq.REQ) 28 | self.cntsocket = self.context.socket(zmq.SUB) 29 | self.socket.connect(f'tcp://{host}:{port}') 30 | self.cntsocket.setsockopt(zmq.SUBSCRIBE, b'') 31 | self.cntsocket.setsockopt(zmq.CONFLATE, 1) 32 | self.cntsocket.connect(f'tcp://{host}:{cntport if cntport else port+1}') 33 | self.uuid = str(uuid.uuid4()) 34 | self.threadLocLevel = 0 35 | 36 | def __del__(self): 37 | """Disconnect and destroy client.""" 38 | self.socket.close() 39 | self.cntsocket.close() 40 | self.context.term() 41 | 42 | def _send(self, req): 43 | if self.verbose > 0: 44 | print('Sending:', req) 45 | rawReq = cbor.dumps(req) 46 | if self.verbose > 1: 47 | print(f'Sending raw len={len(rawReq)}, base64={b64(rawReq)}') 48 | self.socket.send(rawReq) 49 | 50 | def _recv(self): 51 | rawResp = self.socket.recv() 52 | if self.verbose > 1: 53 | print(f'Received raw len={len(rawResp)}, base64={b64(rawResp)}') 54 | resp = cbor.loads(rawResp) 55 | if self.verbose > 0: 56 | print('Received:', resp) 57 | return resp 58 | 59 | def _process_response(self, resp): 60 | if not resp.get('success', False): 61 | raise Exception(resp.get('error')) 62 | ret = resp['ret'] 63 | if len(ret) == 1: 64 | return ret[0] 65 | if len(ret) > 1: 66 | return tuple(ret) 67 | 68 | def call(self, func, args): 69 | """Call function with specified arguments.""" 70 | self._send({'func': func, 'args': args}) 71 | return self._process_response(self._recv()) 72 | 73 | def getObject(self, name, _info=None): 74 | """Retrieve remote object from server.""" 75 | ret = type(name, (), {}) 76 | if not _info: 77 | _info = self.call('zmqRemoteApi.info', [name]) 78 | for k, v in _info.items(): 79 | if not isinstance(v, dict): 80 | raise ValueError('found nondict') 81 | if len(v) == 1 and 'func' in v: 82 | setattr(ret, k, lambda *a, func=f'{name}.{k}': self.call(func, a)) 83 | elif len(v) == 1 and 'const' in v: 84 | setattr(ret, k, v['const']) 85 | else: 86 | setattr(ret, k, self.getObject(f'{name}.{k}', _info=v)) 87 | if name=="sim": 88 | ret.wait=self._wait 89 | ret.waitForSignal=self._waitForSignal 90 | ret.moveToConfig=self._moveToConfig 91 | ret.moveToPose=self._moveToPose 92 | self.sim=ret 93 | return ret 94 | 95 | def setStepping(self, enable=True): 96 | if self.threadLocLevel > 0: 97 | if enable == False: 98 | self.threadLocLevel = 0 99 | return self.call('setStepping', [enable,self.uuid]) 100 | else: 101 | if enable == True: 102 | self.threadLocLevel = 1 103 | return self.call('setStepping', [enable,self.uuid]) 104 | 105 | def step(self, *, wait=True): 106 | if self.threadLocLevel > 0: 107 | self.getStepCount(False) 108 | self.call('step', [self.uuid]) 109 | self.getStepCount(wait) 110 | 111 | def getStepCount(self, wait): 112 | if self.threadLocLevel > 0: 113 | try: 114 | self.cntsocket.recv(0 if wait else zmq.NOBLOCK) 115 | except zmq.ZMQError: 116 | pass 117 | 118 | def _setThreadAutomaticSwitch(self, level): 119 | newLevel = self.threadLocLevel 120 | if isinstance(level,bool): 121 | if level == True: 122 | newLevel -= 1 123 | if newLevel < 0: 124 | newLevel = 0 125 | if level == False: 126 | newLevel += 1 127 | else: 128 | if level >= 0: 129 | newLevel = level 130 | if newLevel != self.threadLocLevel: 131 | if newLevel == 0: 132 | self.setStepping(False) 133 | if newLevel == 1 and self.threadLocLevel == 0: 134 | self.setStepping(True) 135 | self.threadLocLevel = newLevel 136 | return newLevel 137 | 138 | def _wait(self, dt, simTime=True): 139 | lb=self._setThreadAutomaticSwitch(False) 140 | retVal = 0.0 141 | if simTime: 142 | st = self.sim.getSimulationTime() 143 | while (self.sim.getSimulationTime()-st < dt): 144 | self.step() 145 | retVal=self.sim.getSimulationTime()-st-dt 146 | else: 147 | st = self.sim.getSystemTimeInMs(-1) 148 | while (self.sim.getSystemTimeInMs(st) < dt*1000): 149 | self.step() 150 | self._setThreadAutomaticSwitch(lb) 151 | return retVal 152 | 153 | def _waitForSignal(self, sigName): 154 | lb=self._setThreadAutomaticSwitch(False) 155 | retVal = 0.0 156 | while True: 157 | retVal = self.sim.getInt32Signal(sigName)!=None or self.sim.getFloatSignal(sigName)!=None or self.sim.getDoubleSignal(sigName)!=None or self.sim.getStringSignal(sigName)!=None 158 | if retVal: 159 | break 160 | self.step() 161 | self._setThreadAutomaticSwitch(lb) 162 | return retVal 163 | 164 | def _moveToConfig(self, flags,currentPos,currentVel,currentAccel,maxVel,maxAccel,maxJerk,targetPos,targetVel,callback,auxData=None,cyclicJoints=None,timeStep=0): 165 | lb=self._setThreadAutomaticSwitch(False) 166 | 167 | currentPosVelAccel=[] 168 | maxVelAccelJerk=[] 169 | targetPosVel=[] 170 | sel=[] 171 | outPos=[] 172 | outVel=[] 173 | outAccel=[] 174 | for i in range(len(currentPos)): 175 | v=currentPos[i] 176 | currentPosVelAccel.append(v) 177 | outPos.append(v) 178 | maxVelAccelJerk.append(maxVel[i]) 179 | w=targetPos[i] 180 | if cyclicJoints and cyclicJoints[i]: 181 | while w-v>=math.pi*2: 182 | w=w-math.pi*2 183 | while w-v<0: 184 | w=w+math.pi*2 185 | if w-v>math.pi: 186 | w=w-math.pi*2 187 | targetPosVel.append(w) 188 | sel.append(1) 189 | for i in range(len(currentPos)): 190 | if currentVel: 191 | currentPosVelAccel.append(currentVel[i]) 192 | outVel.append(currentVel[i]) 193 | else: 194 | currentPosVelAccel.append(0) 195 | outVel.append(0) 196 | maxVelAccelJerk.append(maxAccel[i]) 197 | if targetVel: 198 | targetPosVel.append(targetVel[i]) 199 | else: 200 | targetPosVel.append(0) 201 | for i in range(len(currentPos)): 202 | if currentAccel: 203 | currentPosVelAccel.append(currentAccel[i]) 204 | outAccel.append(currentAccel[i]) 205 | else: 206 | currentPosVelAccel.append(0) 207 | outAccel.append(0) 208 | maxVelAccelJerk.append(maxJerk[i]) 209 | 210 | if len(maxVel) > len(currentPos): 211 | for i in range(len(maxVel)-len(currentPos)): 212 | currentPosVelAccel.append(maxVel[len(currentPos)+i]) 213 | if len(maxAccel) > len(currentPos): 214 | for i in range(len(maxAccel)-len(currentPos)): 215 | currentPosVelAccel.append(maxAccel[len(currentPos)+i]) 216 | 217 | ruckigObject = self.sim.ruckigPos(len(currentPos),0.0001,flags,currentPosVelAccel,maxVelAccelJerk,sel,targetPosVel) 218 | result = 0 219 | timeLeft = 0 220 | while (result == 0): 221 | dt = timeStep 222 | if dt == 0: 223 | dt=self.sim.getSimulationTimeStep() 224 | syncTime = 0 225 | result,newPosVelAccel,syncTime = self.sim.ruckigStep(ruckigObject,dt) 226 | if result >= 0: 227 | if result == 0: 228 | timeLeft = dt-syncTime 229 | for i in range(len(currentPos)): 230 | outPos[i]=newPosVelAccel[i] 231 | outVel[i]=newPosVelAccel[len(currentPos)+i] 232 | outAccel[i]=newPosVelAccel[len(currentPos)*2+i] 233 | if callback(outPos,outVel,outAccel,auxData): 234 | break 235 | else: 236 | raise RuntimeError("sim.ruckigStep returned error code "+result) 237 | if result == 0: 238 | self.step() 239 | self.sim.ruckigRemove(ruckigObject) 240 | self._setThreadAutomaticSwitch(lb) 241 | return outPos,outVel,outAccel,timeLeft 242 | 243 | def _moveToPose(self, flags,currentPoseOrMatrix,maxVel,maxAccel,maxJerk,targetPoseOrMatrix,callback,auxData=None,metric=None,timeStep=0): 244 | lb = self._setThreadAutomaticSwitch(False) 245 | 246 | usingMatrices = (len(currentPoseOrMatrix)>=12) 247 | if usingMatrices: 248 | currentMatrix = currentPoseOrMatrix 249 | targetMatrix = targetPoseOrMatrix 250 | else: 251 | currentMatrix = self.sim.buildMatrixQ(currentPoseOrMatrix,[currentPoseOrMatrix[3],currentPoseOrMatrix[4],currentPoseOrMatrix[5],currentPoseOrMatrix[6]]) 252 | targetMatrix = self.sim.buildMatrixQ(targetPoseOrMatrix,[targetPoseOrMatrix[3],targetPoseOrMatrix[4],targetPoseOrMatrix[5],targetPoseOrMatrix[6]]) 253 | 254 | outMatrix = self.sim.copyTable(currentMatrix) 255 | axis,angle = self.sim.getRotationAxis(currentMatrix,targetMatrix) 256 | timeLeft = 0 257 | if metric: 258 | # Here we treat the movement as a 1 DoF movement, where we simply interpolate via t between 259 | # the start and goal pose. This always results in straight line movement paths 260 | dx = [(targetMatrix[3]-currentMatrix[3])*metric[0],(targetMatrix[7]-currentMatrix[7])*metric[1],(targetMatrix[11]-currentMatrix[11])*metric[2],angle*metric[3]] 261 | distance = math.sqrt(dx[0]*dx[0]+dx[1]*dx[1]+dx[2]*dx[2]+dx[3]*dx[3]) 262 | if distance > 0.000001: 263 | currentPosVelAccel = [0,0,0] 264 | maxVelAccelJerk = [maxVel[0],maxAccel[0],maxJerk[0]] 265 | if len(maxVel) > 1: 266 | maxVelAccelJerk.append(maxVel[1]) 267 | if len(maxAccel) > 1: 268 | maxVelAccelJerk.append(maxAccel[1]) 269 | targetPosVel = [distance,0] 270 | ruckigObject = self.sim.ruckigPos(1,0.0001,flags,currentPosVelAccel,maxVelAccelJerk,[1],targetPosVel) 271 | result = 0 272 | while (result == 0): 273 | dt = timeStep 274 | if dt == 0: 275 | dt = self.sim.getSimulationTimeStep() 276 | result,newPosVelAccel,syncTime = self.sim.ruckigStep(ruckigObject,dt) 277 | if result >= 0: 278 | if result == 0: 279 | timeLeft = dt-syncTime 280 | t = newPosVelAccel[0]/distance 281 | outMatrix = self.sim.interpolateMatrices(currentMatrix,targetMatrix,t) 282 | nv = [newPosVelAccel[1]] 283 | na = [newPosVelAccel[2]] 284 | if not usingMatrices: 285 | q = self.sim.getQuaternionFromMatrix(outMatrix) 286 | outMatrix = [outMatrix[3],outMatrix[7],outMatrix[11],q[0],q[1],q[2],q[3]] 287 | if callback(outMatrix,nv,na,auxData): 288 | break 289 | else: 290 | raise RuntimeError("sim.ruckigStep returned error code "+result) 291 | if result == 0: 292 | self.step() 293 | self.sim.ruckigRemove(ruckigObject) 294 | else: 295 | # Here we treat the movement as a 4 DoF movement, where each of X, Y, Z and rotation 296 | # is handled and controlled individually. This can result in non-straight line movement paths, 297 | # due to how the Ruckig functions operate depending on 'flags' 298 | dx = [targetMatrix[3]-currentMatrix[3],targetMatrix[7]-currentMatrix[7],targetMatrix[11]-currentMatrix[11],angle] 299 | currentPosVelAccel = [0,0,0,0,0,0,0,0,0,0,0,0] 300 | maxVelAccelJerk = [maxVel[0],maxVel[1],maxVel[2],maxVel[3],maxAccel[0],maxAccel[1],maxAccel[2],maxAccel[3],maxJerk[0],maxJerk[1],maxJerk[2],maxJerk[3]] 301 | if len(maxVel) > 4: 302 | for i in range(len(maxVel)-len(maxJerk)): 303 | maxVelAccelJerk.append(maxVel[len(maxJerk)+i]) 304 | if len(maxAccel) > 4: 305 | for i in range(len(maxAccel)-len(maxJerk)): 306 | maxVelAccelJerk.append(maxAccel[len(maxJerk)+i]) 307 | targetPosVel = [dx[0],dx[1],dx[2],dx[3],0,0,0,0,0] 308 | ruckigObject = self.sim.ruckigPos(4,0.0001,flags,currentPosVelAccel,maxVelAccelJerk,[1,1,1,1],targetPosVel) 309 | result = 0 310 | while (result == 0): 311 | dt = timeStep 312 | if dt == 0: 313 | dt = self.sim.getSimulationTimeStep() 314 | result,newPosVelAccel,syncTime = self.sim.ruckigStep(ruckigObject,dt) 315 | if result >= 0: 316 | if result == 0: 317 | timeLeft = dt-syncTime 318 | t = 0 319 | if abs(angle)>math.pi*0.00001: 320 | t = newPosVelAccel[3]/angle 321 | outMatrix = self.sim.interpolateMatrices(currentMatrix,targetMatrix,t) 322 | outMatrix[3] = currentMatrix[3]+newPosVelAccel[0] 323 | outMatrix[7] = currentMatrix[7]+newPosVelAccel[1] 324 | outMatrix[11] = currentMatrix[11]+newPosVelAccel[2] 325 | nv = [newPosVelAccel[4],newPosVelAccel[5],newPosVelAccel[6],newPosVelAccel[7]] 326 | na = [newPosVelAccel[8],newPosVelAccel[9],newPosVelAccel[10],newPosVelAccel[11]] 327 | if not usingMatrices: 328 | q = self.sim.getQuaternionFromMatrix(outMatrix) 329 | outMatrix = [outMatrix[3],outMatrix[7],outMatrix[11],q[0],q[1],q[2],q[3]] 330 | if callback(outMatrix,nv,na,auxData): 331 | break 332 | else: 333 | raise RuntimeError("sim.ruckigStep returned error code "+result) 334 | if result == 0: 335 | self.step() 336 | self.sim.ruckigRemove(ruckigObject) 337 | 338 | self._setThreadAutomaticSwitch(lb) 339 | return outMatrix,timeLeft 340 | 341 | 342 | if __name__ == '__console__': 343 | client = RemoteAPIClient() 344 | sim = client.getObject('sim') 345 | 346 | 347 | __all__ = ['RemoteAPIClient'] 348 | --------------------------------------------------------------------------------