V-REP和Python的联合仿真

机器人仿真软件 各类免费的的机器人仿真软件优缺点汇总_robot 仿真 软件收费么_dyannacon的博客-CSDN博客

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

课程资料 guyueclass: 古月学院课程代码

旋转变换 旋转的左乘与右乘 - 知乎

四足机器人站立控制原理 【基础知识】四足机器人的站立姿态控制原理 - 知乎

单腿逆解参考 https://github.com/richardbloemenkamp/Robotdog

Vrep文档

Vrep放大object

Vrep 导入模型步骤:

1. plugins-->urdf import导入机器人URDF文件

2. 删除机器人对象中的world_joint和world_link_visual

3. 双击设置机器人参数

碰撞参数设置:body参数设置,自身碰撞勾选前四个勾,leg参数设置,自身碰撞勾选后四个勾,即不计算与自身的碰撞关系

设置关节参数

调节颜色

python联合仿真

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

1. 选择仿真器

2. 创建Vrep脚本用于远程连接

3. 绑定脚本到机器人

4. 编辑脚本,添加远程连接代码

4. 编写python脚本并测试(将腿部足端位置转换为关节的角度)

连接V-REP需要从remote API路径拷贝相关文件

"""
连接VREP Server并测试控制四足机器人
"""
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 = None

import time
import numpy as np


def start_simulation():
    sim.simxFinish(-1)
    # 开启套接字与server进行通信
    clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
    if clientID != -1:
        print('Connected to remote API server with ClientID ', clientID)
        # 开始模拟
        sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)

        return clientID
    else:
        return -1


def get_joints(client_id):
    # 机器人电机力矩参数
    rotation_forces = [
        # RB
        [500, 500, 500],
        # RF
        [500, 500, 500],
        # LB
        [500, 500, 500],
        # LF
        [500, 500, 500]
    ]

    # 获取机器人关节对象句柄
    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)

    # 设置电机力矩
    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分别是单条腿杆件的长度
    h = 0.15
    hu = 0.35
    hl = 0.382
    dyz = 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_offset

    lxzp = 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_off

    return gamma, alfa, beta


if __name__ == '__main__':
    # 机器人电机角度参数
    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 = 60
        start_time = time.time()
        curr_time = time.time()

        # 初始关节角度
        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:
            # 设置关节角度
            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. 打开shape编辑模式,并在vertex编辑模式下选择节点,在添加dummy

将dummy移动到腿部object下

2. 添加图用于创建curve

3. 设置3D Curve

4. 修改位置控制速度上限(将速度上限修改为500)

步态控制

utils.py

import sim
import numpy as np


def start_simulation():
    sim.simxFinish(-1)
    # 开启套接字与server进行通信
    clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
    if clientID != -1:
        print('Connected to remote API server with ClientID ', clientID)
        # 开始模拟
        sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)

        return clientID
    else:
        return -1


def get_joints(client_id):
    # 机器人电机力矩参数
    rotation_forces = [
        # RB
        [500, 500, 500],
        # RF
        [500, 500, 500],
        # LB
        [500, 500, 500],
        # LF
        [500, 500, 500]
    ]

    # 获取机器人关节对象句柄
    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)

    # 设置电机力矩
    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分别是单条腿杆件的长度
    h = 0.15
    hu = 0.35
    hl = 0.382
    dyz = 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_offset

    lxzp = 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_off

    return gamma, alfa, beta


def pose_control(roll, pitch, yaw, pos_x, pos_y, pos_z):
    """
    输入
    """
    b = 0.4
    l = 0.8
    w = 0.7
    # 基座的高度
    h = 0.732

    # 转换角度
    R = roll * np.pi / 180
    P = pitch * np.pi / 180
    Y = yaw * np.pi / 180

    pos = np.mat([pos_x, pos_y, pos_z]).T

    # 定义旋转矩阵
    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]]).T

    leg_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):
    """
    计算摆线上在给定时间t处的坐标。

    参数:
    t (float): 当前时间点
    Ts (float): 摆线运动总时间,默认为1.0
    xs (float): 起始x坐标,默认为-0.1
    xf (float): 终点x坐标,默认为0.1
    zs (float): 起始z坐标,默认为-0.582
    h (float): 摆线垂直位移,默认为0.1

    返回:
    tuple[float, float]: xep和zep的坐标值
    """
    sigma = 2 * np.pi * dt / period
    x_p = (xf - xs) * ((sigma - np.sin(sigma)) / (2 * np.pi)) + xs
    y_p = h * (1 - np.cos(sigma)) / 2 + zs
    return x_p, y_p


if __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.4

gait = 1


def 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), zs


def walk_gait(dt):
    zs = -0.482
    h = 0.15

    lb_dt = dt % walk_period
    rf_dt = (dt + 0.25) % walk_period
    rb_dt = (dt + 0.5) % walk_period
    lf_dt = (dt + 0.75) % walk_period

    lb_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_pos


def trot_gait(dt):
    zs = -0.482
    h = 0.1

    dt_1 = dt % trot_period
    dt_2 = (dt + 0.2) % trot_period

    pos_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_2


if __name__ == '__main__':
    # 连接到V-REP服务器
    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 = 60
        start_time = time.time()
        curr_time = start_time

        sim_start_time, sim_curr_time = None, None
        lb_pos, rf_pos, rb_pos, lf_pos = None, None, None, None

        # 获取仿真时间
        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_time

                print("time ", sim_curr_time - sim_start_time)

            if sim_start_time:
                dt = sim_curr_time - sim_start_time
                if gait == 0:
                    # dt = (sim_curr_time - sim_start_time) % walk_period
                    lb_pos, rf_pos, rb_pos, lf_pos = walk_gait(dt)
                elif gait == 1:
                    # dt = (sim_curr_time - sim_start_time) % trot_period
                    pos_1, pos_2 = trot_gait(dt)
                    lb_pos = pos_1
                    rf_pos = pos_1
                    rb_pos = pos_2
                    lf_pos = pos_2

                # 从足端位置求解关节角度
                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)

        # 停止仿真并断开与V-REP的连接
        sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
        sim.simxFinish(clientID)

    else:
        print("无法连接到V-REP")

walk步态

trot步态

  • 1
    点赞
  • 4
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值