機(jī)器人仿真軟件 各類(lèi)免費(fèi)的的機(jī)器人仿真軟件優(yōu)缺點(diǎn)匯總_robot 仿真 軟件收費(fèi)么_dyannacon的博客-CSDN博客

課程地址 https://class.guyuehome.com/p/t_pc/course_pc_detail/column/p_605af87be4b007b4183a42e7

課程資料 guyueclass: 古月學(xué)院課程代碼

旋轉(zhuǎn)變換 旋轉(zhuǎn)的左乘與右乘 - 知乎

四足機(jī)器人站立控制原理 【基礎(chǔ)知識(shí)】四足機(jī)器人的站立姿態(tài)控制原理 - 知乎

單腿逆解參考 https://github.com/richardbloemenkamp/Robotdog

Vrep文檔

Vrep放大object

Vrep 導(dǎo)入模型步驟：

1. plugins-->urdf import導(dǎo)入機(jī)器人URDF文件

2. 刪除機(jī)器人對(duì)象中的world_joint和world_link_visual

3. 雙擊設(shè)置機(jī)器人參數(shù)

碰撞參數(shù)設(shè)置：body參數(shù)設(shè)置，自身碰撞勾選前四個(gè)勾，leg參數(shù)設(shè)置，自身碰撞勾選后四個(gè)勾，即不計(jì)算與自身的碰撞關(guān)系

設(shè)置關(guān)節(jié)參數(shù)

調(diào)節(jié)顏色

python聯(lián)合仿真

remote API路徑：C:\Program Files\CoppeliaRobotics\CoppeliaSimEdu\programming\remoteApiBindings

1. 選擇仿真器

2. 創(chuàng)建Vrep腳本用于遠(yuǎn)程連接

3. 綁定腳本到機(jī)器人

4. 編輯腳本，添加遠(yuǎn)程連接代碼

4. 編寫(xiě)python腳本并測(cè)試（將腿部足端位置轉(zhuǎn)換為關(guān)節(jié)的角度）

連接V-REP需要從remote API路徑拷貝相關(guān)文件

"""
連接VREP Server并測(cè)試控制四足機(jī)器人
"""
try:import sim
except ImportError:print('--------------------------------------------------------------')print('"sim.py" could not be imported. This means very probably that')print('either "sim.py" or the remoteApi library could not be found.')print('Make sure both are in the same folder as this file,')print('or appropriately adjust the file "sim.py"')print('--------------------------------------------------------------')print('')sim = Noneimport time
import numpy as npdef start_simulation():sim.simxFinish(-1)# 開(kāi)啟套接字與server進(jìn)行通信clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)if clientID != -1:print('Connected to remote API server with ClientID ', clientID)# 開(kāi)始模擬sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)return clientIDelse:return -1def get_joints(client_id):# 機(jī)器人電機(jī)力矩參數(shù)rotation_forces = [# RB[500, 500, 500],# RF[500, 500, 500],# LB[500, 500, 500],# LF[500, 500, 500]]# 獲取機(jī)器人關(guān)節(jié)對(duì)象句柄rec, rb_rot_1 = sim.simxGetObjectHandle(client_id, 'rb_rot_1', sim.simx_opmode_blocking)rec, rb_rot_2 = sim.simxGetObjectHandle(client_id, 'rb_rot_2', sim.simx_opmode_blocking)rec, rb_rot_3 = sim.simxGetObjectHandle(client_id, 'rb_rot_3', sim.simx_opmode_blocking)rec, rf_rot_1 = sim.simxGetObjectHandle(client_id, 'rf_rot_1', sim.simx_opmode_blocking)rec, rf_rot_2 = sim.simxGetObjectHandle(client_id, 'rf_rot_2', sim.simx_opmode_blocking)rec, rf_rot_3 = sim.simxGetObjectHandle(client_id, 'rf_rot_3', sim.simx_opmode_blocking)rec, lb_rot_1 = sim.simxGetObjectHandle(client_id, 'lb_rot_1', sim.simx_opmode_blocking)rec, lb_rot_2 = sim.simxGetObjectHandle(client_id, 'lb_rot_2', sim.simx_opmode_blocking)rec, lb_rot_3 = sim.simxGetObjectHandle(client_id, 'lb_rot_3', sim.simx_opmode_blocking)rec, lf_rot_1 = sim.simxGetObjectHandle(client_id, 'lf_rot_1', sim.simx_opmode_blocking)rec, lf_rot_2 = sim.simxGetObjectHandle(client_id, 'lf_rot_2', sim.simx_opmode_blocking)rec, lf_rot_3 = sim.simxGetObjectHandle(client_id, 'lf_rot_3', sim.simx_opmode_blocking)# 設(shè)置電機(jī)力矩rec = sim.simxSetJointForce(client_id, rb_rot_1, rotation_forces[0][0], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, rb_rot_2, rotation_forces[0][1], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, rb_rot_3, rotation_forces[0][2], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, rf_rot_1, rotation_forces[1][0], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, rf_rot_2, rotation_forces[1][1], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, rf_rot_3, rotation_forces[1][2], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, lb_rot_1, rotation_forces[2][0], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, lb_rot_2, rotation_forces[2][1], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, lb_rot_3, rotation_forces[2][2], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, lf_rot_1, rotation_forces[3][0], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, lf_rot_2, rotation_forces[3][1], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, lf_rot_3, rotation_forces[3][2], sim.simx_opmode_blocking)return [rb_rot_1, rb_rot_2, rb_rot_3], \[rf_rot_1, rf_rot_2, rf_rot_3], \[lb_rot_1, lb_rot_2, lb_rot_3], \[lf_rot_1, lf_rot_2, lf_rot_3]def leg_inverse_kine(x, y, z):# h,hu和hl分別是單條腿桿件的長(zhǎng)度h = 0.15hu = 0.35hl = 0.382dyz = np.sqrt(y**2 + z**2)lyz = np.sqrt(dyz**2 - h**2)gamma_yz = -np.arctan(y/z)gamma_h_offset = -np.arctan(h/lyz)gamma = gamma_yz - gamma_h_offsetlxzp = np.sqrt(lyz**2 + x**2)n = (lxzp**2 - hl**2 - hu**2) / (2 * hu)beta = -np.arccos(n / hl)alfa_xzp = -np.arctan(x/lyz)alfa_off = np.arccos((hu + n) / lxzp)alfa = alfa_xzp + alfa_offreturn gamma, alfa, betaif __name__ == '__main__':# 機(jī)器人電機(jī)角度參數(shù)rb_poses = [40*np.pi/180, 0, 0]rf_poses = [0, 0, 0]lb_poses = [0, 0, 0]lf_poses = [0, 0, 0]client_id = start_simulation()if client_id != -1:joints = get_joints(client_id)rb_joints = joints[0]rf_joints = joints[1]lb_joints = joints[2]lf_joints = joints[3]time.sleep(1)timeout = 60start_time = time.time()curr_time = time.time()# 初始關(guān)節(jié)角度rb_poses = leg_inverse_kine(0, -0.3, -0.632)rf_poses = leg_inverse_kine(0, -0.3, -0.632)lb_poses = leg_inverse_kine(0, -0.3, -0.632)lf_poses = leg_inverse_kine(0, -0.3, -0.632)while curr_time - start_time < timeout:# 設(shè)置關(guān)節(jié)角度rec = sim.simxSetJointTargetPosition(client_id, rb_joints[0], -rb_poses[0], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(client_id, rb_joints[1], rb_poses[1], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(client_id, rb_joints[2], rb_poses[2], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(client_id, rf_joints[0], rf_poses[0], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(client_id, rf_joints[1], rf_poses[1], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(client_id, rf_joints[2], rf_poses[2], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(client_id, lb_joints[0], -lb_poses[0], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(client_id, lb_joints[1], lb_poses[1], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(client_id, lb_joints[2], lb_poses[2], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(client_id, lf_joints[0], lf_poses[0], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(client_id, lf_joints[1], lf_poses[1], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(client_id, lf_joints[2], lf_poses[2], sim.simx_opmode_oneshot)curr_time = time.time()# print("curr time :", curr_time - start_time)# 完成模擬sim.simxStopSimulation(client_id, sim.simx_opmode_blocking)sim.simxFinish(client_id)else:print('Failed connecting to remote API server')

顯示足端軌跡

1. 打開(kāi)shape編輯模式，并在vertex編輯模式下選擇節(jié)點(diǎn)，在添加dummy

將dummy移動(dòng)到腿部object下

2. 添加圖用于創(chuàng)建curve

3. 設(shè)置3D Curve

4. 修改位置控制速度上限（將速度上限修改為500）

步態(tài)控制

utils.py

import sim
import numpy as npdef start_simulation():sim.simxFinish(-1)# 開(kāi)啟套接字與server進(jìn)行通信clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)if clientID != -1:print('Connected to remote API server with ClientID ', clientID)# 開(kāi)始模擬sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)return clientIDelse:return -1def get_joints(client_id):# 機(jī)器人電機(jī)力矩參數(shù)rotation_forces = [# RB[500, 500, 500],# RF[500, 500, 500],# LB[500, 500, 500],# LF[500, 500, 500]]# 獲取機(jī)器人關(guān)節(jié)對(duì)象句柄rec, rb_rot_1 = sim.simxGetObjectHandle(client_id, 'rb_rot_1', sim.simx_opmode_blocking)rec, rb_rot_2 = sim.simxGetObjectHandle(client_id, 'rb_rot_2', sim.simx_opmode_blocking)rec, rb_rot_3 = sim.simxGetObjectHandle(client_id, 'rb_rot_3', sim.simx_opmode_blocking)rec, rf_rot_1 = sim.simxGetObjectHandle(client_id, 'rf_rot_1', sim.simx_opmode_blocking)rec, rf_rot_2 = sim.simxGetObjectHandle(client_id, 'rf_rot_2', sim.simx_opmode_blocking)rec, rf_rot_3 = sim.simxGetObjectHandle(client_id, 'rf_rot_3', sim.simx_opmode_blocking)rec, lb_rot_1 = sim.simxGetObjectHandle(client_id, 'lb_rot_1', sim.simx_opmode_blocking)rec, lb_rot_2 = sim.simxGetObjectHandle(client_id, 'lb_rot_2', sim.simx_opmode_blocking)rec, lb_rot_3 = sim.simxGetObjectHandle(client_id, 'lb_rot_3', sim.simx_opmode_blocking)rec, lf_rot_1 = sim.simxGetObjectHandle(client_id, 'lf_rot_1', sim.simx_opmode_blocking)rec, lf_rot_2 = sim.simxGetObjectHandle(client_id, 'lf_rot_2', sim.simx_opmode_blocking)rec, lf_rot_3 = sim.simxGetObjectHandle(client_id, 'lf_rot_3', sim.simx_opmode_blocking)# 設(shè)置電機(jī)力矩rec = sim.simxSetJointForce(client_id, rb_rot_1, rotation_forces[0][0], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, rb_rot_2, rotation_forces[0][1], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, rb_rot_3, rotation_forces[0][2], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, rf_rot_1, rotation_forces[1][0], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, rf_rot_2, rotation_forces[1][1], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, rf_rot_3, rotation_forces[1][2], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, lb_rot_1, rotation_forces[2][0], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, lb_rot_2, rotation_forces[2][1], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, lb_rot_3, rotation_forces[2][2], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, lf_rot_1, rotation_forces[3][0], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, lf_rot_2, rotation_forces[3][1], sim.simx_opmode_blocking)rec = sim.simxSetJointForce(client_id, lf_rot_3, rotation_forces[3][2], sim.simx_opmode_blocking)return [rb_rot_1, rb_rot_2, rb_rot_3], \[rf_rot_1, rf_rot_2, rf_rot_3], \[lb_rot_1, lb_rot_2, lb_rot_3], \[lf_rot_1, lf_rot_2, lf_rot_3]def leg_inverse_kine(x, y, z):"""求四足機(jī)器人單條腿的逆運(yùn)動(dòng)學(xué)，輸入足端位置，返回單腿關(guān)節(jié)的旋轉(zhuǎn)的角度"""# h,hu和hl分別是單條腿桿件的長(zhǎng)度h = 0.15hu = 0.35hl = 0.382dyz = np.sqrt(y ** 2 + z ** 2)lyz = np.sqrt(dyz ** 2 - h ** 2)gamma_yz = -np.arctan(y / z)gamma_h_offset = -np.arctan(h / lyz)gamma = gamma_yz - gamma_h_offsetlxzp = np.sqrt(lyz ** 2 + x ** 2)n = (lxzp ** 2 - hl ** 2 - hu ** 2) / (2 * hu)beta = -np.arccos(n / hl)alfa_xzp = -np.arctan(x / lyz)alfa_off = np.arccos((hu + n) / lxzp)alfa = alfa_xzp + alfa_offreturn gamma, alfa, betadef pose_control(roll, pitch, yaw, pos_x, pos_y, pos_z):"""輸入"""b = 0.4l = 0.8w = 0.7# 基座的高度h = 0.732# 轉(zhuǎn)換角度R = roll * np.pi / 180P = pitch * np.pi / 180Y = yaw * np.pi / 180pos = np.mat([pos_x, pos_y, pos_z]).T# 定義旋轉(zhuǎn)矩陣rotx = np.mat([[1, 0, 0],[0, np.cos(R), -np.sin(R)],[0, np.sin(R), np.cos(R)]])roty = np.mat([[np.cos(P), 0, -np.sin(P)],[0, 1, 0],[np.sin(P), 0, np.cos(P)]])rotz = np.mat([[np.cos(Y), -np.sin(Y), 0],[np.sin(Y), np.cos(Y), 0],[0, 0, 1]])rot_mat = rotx * roty * rotz# 基座位置body_struct = np.mat([[l / 2, b / 2, h],[l / 2, -b / 2, h],[-l / 2, b / 2, h],[-l / 2, -b / 2, h]]).T# 足端位置footpoint_struct = np.mat([[l / 2, w / 2, 0],[l / 2, -w / 2, 0],[-l / 2, w / 2, 0],[-l / 2, -w / 2, 0]]).Tleg_pose = np.mat(np.zeros((3, 4)))for i in range(4):leg_pose[:, i] = -pos - rot_mat * body_struct[:, i] + footpoint_struct[:, i]return np.squeeze(np.array(leg_pose[:, 3])), np.squeeze(np.array(leg_pose[:, 0])), \np.squeeze(np.array(leg_pose[:, 1])), np.squeeze(np.array(leg_pose[:, 2]))def cycloid(dt: float, period: float = 1.0, xs: float = -0.1, xf: float = 0.1, zs: float = -0.582, h: float = 0.1):"""計(jì)算擺線(xiàn)上在給定時(shí)間t處的坐標(biāo)。參數(shù)：t (float): 當(dāng)前時(shí)間點(diǎn)Ts (float): 擺線(xiàn)運(yùn)動(dòng)總時(shí)間，默認(rèn)為1.0xs (float): 起始x坐標(biāo)，默認(rèn)為-0.1xf (float): 終點(diǎn)x坐標(biāo)，默認(rèn)為0.1zs (float): 起始z坐標(biāo)，默認(rèn)為-0.582h (float): 擺線(xiàn)垂直位移，默認(rèn)為0.1返回：tuple[float, float]: xep和zep的坐標(biāo)值"""sigma = 2 * np.pi * dt / periodx_p = (xf - xs) * ((sigma - np.sin(sigma)) / (2 * np.pi)) + xsy_p = h * (1 - np.cos(sigma)) / 2 + zsreturn x_p, y_pif __name__ == '__main__':for pos in pose_control(30, 0, 0, 0, 0, 0.732):print(pos)

main.py

import time
from utils import *walk_period = 1.0
trot_period = 0.4gait = 1def cal_phase(dt, T, factor, zs = -0.482, h = 0.15):if dt < T * factor:return cycloid(dt, period=T * factor, zs=zs, h=h)else:return 0.1 - 0.2 / (T * (1 - factor)) * (dt - T * factor), zsdef walk_gait(dt):zs = -0.482h = 0.15lb_dt = dt % walk_periodrf_dt = (dt + 0.25) % walk_periodrb_dt = (dt + 0.5) % walk_periodlf_dt = (dt + 0.75) % walk_periodlb_pos = cal_phase(lb_dt, T=walk_period, factor=0.25, zs=zs, h=h)rf_pos = cal_phase(rf_dt, T=walk_period, factor=0.25, zs=zs, h=h)rb_pos = cal_phase(rb_dt, T=walk_period, factor=0.25, zs=zs, h=h)lf_pos = cal_phase(lf_dt, T=walk_period, factor=0.25, zs=zs, h=h)return lb_pos, rf_pos, rb_pos, lf_posdef trot_gait(dt):zs = -0.482h = 0.1dt_1 = dt % trot_perioddt_2 = (dt + 0.2) % trot_periodpos_1 = cal_phase(dt_1, T=trot_period, factor=0.5, zs=zs, h=h)pos_2 = cal_phase(dt_2, T=trot_period, factor=0.5, zs=zs, h=h)return pos_1, pos_2if __name__ == '__main__':# 連接到V-REP服務(wù)器clientID = start_simulation()# 檢查連接是否成功if clientID != -1:joints = get_joints(clientID)rb_joints = joints[0]rf_joints = joints[1]lb_joints = joints[2]lf_joints = joints[3]timeout = 60start_time = time.time()curr_time = start_timesim_start_time, sim_curr_time = None, Nonelb_pos, rf_pos, rb_pos, lf_pos = None, None, None, None# 獲取仿真時(shí)間while curr_time - start_time < timeout:res, sim_curr_time = sim.simxGetFloatSignal(clientID, 'time', sim.simx_opmode_oneshot)if res == sim.simx_return_ok:if sim_start_time is None:sim_start_time = sim_curr_timeprint("time ", sim_curr_time - sim_start_time)if sim_start_time:dt = sim_curr_time - sim_start_timeif gait == 0:# dt = (sim_curr_time - sim_start_time) % walk_periodlb_pos, rf_pos, rb_pos, lf_pos = walk_gait(dt)elif gait == 1:# dt = (sim_curr_time - sim_start_time) % trot_periodpos_1, pos_2 = trot_gait(dt)lb_pos = pos_1rf_pos = pos_1rb_pos = pos_2lf_pos = pos_2# 從足端位置求解關(guān)節(jié)角度rb_pose = leg_inverse_kine(rb_pos[0], -0.15, rb_pos[1])rf_pose = leg_inverse_kine(rf_pos[0], -0.15, rf_pos[1])lb_pose = leg_inverse_kine(lb_pos[0], -0.15, lb_pos[1])lf_pose = leg_inverse_kine(lf_pos[0], -0.15, lf_pos[1])rec = sim.simxSetJointTargetPosition(clientID, rb_joints[0], -rb_pose[0], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(clientID, rb_joints[1], rb_pose[1], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(clientID, rb_joints[2], rb_pose[2], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(clientID, rf_joints[0], rf_pose[0], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(clientID, rf_joints[1], rf_pose[1], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(clientID, rf_joints[2], rf_pose[2], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(clientID, lb_joints[0], -lb_pose[0], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(clientID, lb_joints[1], lb_pose[1], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(clientID, lb_joints[2], lb_pose[2], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(clientID, lf_joints[0], lf_pose[0], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(clientID, lf_joints[1], lf_pose[1], sim.simx_opmode_oneshot)rec = sim.simxSetJointTargetPosition(clientID, lf_joints[2], lf_pose[2], sim.simx_opmode_oneshot)# 停止仿真并斷開(kāi)與V-REP的連接sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)sim.simxFinish(clientID)else:print("無(wú)法連接到V-REP")

walk步態(tài)

trot步態(tài)