仿生蛇形机器人04、Dynamixel MX-64AR舵机串联实现仿生基本步态

发布于:2022-11-09 ⋅ 阅读:(5) ⋅ 点赞:(0) ⋅ 评论:(0)

一、3D结构

外观通过solidworks建模,用大昆3d打印机打印。

关节展示

在这里插入图片描述

二、实现原理

首先要看过我的第一篇文章,知道demo的代码功能,如下图。
图中上面三个值,可以理解为选型,就是选择我这个舵机的类型,这个值可以在我再第一篇文章里面介绍的上位机看到。

仿生蛇形机器人01、Dynamixel MX-64AR舵机控制例程的使用【Python 1.0协议】

在这里插入图片描述

这个三个值就是主要控制舵机的参数

在这里插入图片描述

read_write.py这个程序就是从大约为0°位置转到360°,再转回来。
而0 ~ 4096 就是对应 0 ~ 360°,所以可以有下面的理解:

在这里插入图片描述

这是一个PWM脉宽调制,相信有接触过的人都不会陌生。这个就是对应着这个舵机的,从在数值10对应的角度开始,在得到高电平的时候进行旋转到指定位置,每次增加20数值对应的角度,直到达到4000,再按下一次回车就是反转回去。

下面我来说说仿生蛇,首先就是舵机横纵交错连接,是一种运动的结构。只要对控制PWM的输出口输入sinwt的正弦信号,控制转动和正反转,PWM会随着时间的变化而变化,这样就实现了舵机的一种控制,这就是仿生蛇的步态控制,也是从外国的论文文献中看到的。如下图。

在这里插入图片描述

根据参数的不同,就会产生不同的步态。

三、代码实现

目前只是加了六个舵机,后面如果继续研究会继续加舵机的。

我之前说用Atom,我后来发现随便用什么的python编译器都可以,这里我之后改用了pycharm编译器。

给大家展示下python代码,供给后来人学习参考,对了我是windows系统上使用的,可能其他系统也会有差别,还有就是这个舵机的通信是写在底层的,我是直接使用的。

大家可以对比官方给的例程read_write.py参考,我这个是在例程demo中直接修改编写的。

3_test_6_2.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

################################################################################
# Copyright 2017 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
################################################################################

# Author: Ryu Woon Jung (Leon)

#
# *********     Read and Write Example      *********
#
#
# Available DXL model on this example : All models using Protocol 1.0
# This example is tested with a DXL MX-28, and an USB2DYNAMIXEL
# Be sure that DXL MX properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600)
#

import os
import numpy as np

if os.name == 'nt':
    import msvcrt


    def getch():
        return msvcrt.getch().decode()
else:
    import sys, tty, termios

    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)


    def getch():
        try:
            tty.setraw(sys.stdin.fileno())
            ch = sys.stdin.read(1)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        return ch

from dynamixel_sdk import *  # Uses Dynamixel SDK library
import math
import time


def sin_fun(phi, t, T, A=math.pi / 4, B=math.pi / 2):
    '''
    input : A, T, phi, t, B : Amplitude Period Phase Time Offset
    return : theta (0-360 Convert to 0-4096)
    '''
    w = 2 * math.pi / T
    theta = A * math.sin(w * t - phi) + B
    theta = int(theta / math.pi * 0.5 * 4096)
    return theta


if __name__ == '__main__':
    # Control table address
    ADDR_MX_TORQUE_ENABLE = 24  # Control table address is different in Dynamixel model
    ADDR_MX_GOAL_POSITION = 30
    ADDR_MX_PRESENT_POSITION = 36

    # Protocol version
    PROTOCOL_VERSION = 1.0  # See which protocol version is used in the Dynamixel

    # Default setting
    servo_num = 6
    DXL_ID = [3,4,5,6,7,8]  # Dynamixel ID
    BAUDRATE = 57600  # Dynamixel default baudrate : 57600
    DEVICENAME = 'COM13'  # Check which port is being used on your controller
    # ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"

    TORQUE_ENABLE = 1  # Value for enabling the torque
    TORQUE_DISABLE = 0  # Value for disabling the torque

    DXL_MOVING_STATUS_THRESHOLD = []  # Dynamixel moving status threshold
    for i in range(servo_num):
        DXL_MOVING_STATUS_THRESHOLD.append(20)

    # Initialize PortHandler instance
    # Set the port path
    # Get methods and members of PortHandlerLinux or PortHandlerWindows
    portHandler = PortHandler(DEVICENAME)

    # Initialize PacketHandler instance
    # Set the protocol version
    # Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
    packetHandler = PacketHandler(PROTOCOL_VERSION)

    # Open port
    if portHandler.openPort():
        print("Succeeded to open the port")
    else:
        print("Failed to open the port")
        print("Press any key to terminate...")
        getch()
        quit()

    # Set port baudrate
    if portHandler.setBaudRate(BAUDRATE):
        print("Succeeded to change the baudrate")
    else:
        print("Failed to change the baudrate")
        print("Press any key to terminate...")
        getch()
        quit()

    # Enable Dynamixel Torque
    for i in range(servo_num):
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID[i], ADDR_MX_TORQUE_ENABLE,
                                                                  TORQUE_ENABLE)
        if dxl_comm_result != COMM_SUCCESS:
            print("##### Enable Dynamixel Torque", "%s" % packetHandler.getTxRxResult(dxl_comm_result))
        elif dxl_error != 0:
            print("##### Enable Dynamixel Torque", "%s" % packetHandler.getRxPacketError(dxl_error))
        else:
            print("Dynamixel " + str(i) + " has been successfully connected")

    # Define parameters
    T = 2  # T > 1, w < 2*pi/1s
    phi = [0, 0, math.pi / 6, math.pi / 6, math.pi / 3, math.pi / 3, math.pi / 2, math.pi / 2]
    start = 0
    end = 100
    dxl_goal_position = []
    dxl_present_position = []
    for i in range(servo_num):
        dxl_goal_position.append(0)
        dxl_present_position.append(0)

    # ----------------------------------------------------------------------------
    # Set initial position

    for i in range(servo_num):
        dxl_goal_position[i] = sin_fun(phi=phi[i], t=start, T=T)
    print('initial position = ', dxl_goal_position)
    for i in range(servo_num):
        dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID[i], ADDR_MX_GOAL_POSITION,
                                                                  dxl_goal_position[i])
        if dxl_comm_result != COMM_SUCCESS:
            print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
        elif dxl_error != 0:
            print("%s" % packetHandler.getRxPacketError(dxl_error))
    judge = []
    for i in range(servo_num):
        judge.append('1')
    while True:
        # Read present position
        for i in range(servo_num):
            dxl_present_position[i], dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL_ID[i],
                                                                                              ADDR_MX_PRESENT_POSITION)
            if dxl_comm_result != COMM_SUCCESS:
                print("%s" % packetHandler.getTxRxResult(dxl_comm_result), "servo_num is :", i + 1)
            elif dxl_error != 0:
                print("%s" % packetHandler.getRxPacketError(dxl_error), "servo_num is :", i + 1)
            if abs(dxl_goal_position[i] - dxl_present_position[i]) < DXL_MOVING_STATUS_THRESHOLD[i]:
                judge[i] = '0'
            else:
                dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID[i], ADDR_MX_GOAL_POSITION,
                                                                          dxl_goal_position[i])
                if dxl_comm_result != COMM_SUCCESS:
                    print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
                elif dxl_error != 0:
                    print("%s" % packetHandler.getRxPacketError(dxl_error))
        print('Initial present position=', dxl_present_position)
        print('Initial goal_position=', dxl_goal_position)
        print('judge=', judge)
        if '1' not in judge:
            break

    # -------------------------------------------------------------------------------------------------
    print("Press any key to continue! (or press ESC to end the program !)")
    # if getch() == chr(0x1b):
    #     import sys
    #
    #     sys.exit(0)
    servo_time = np.arange(start, end, 0.1)
    for t in servo_time:
        # Calculate goal position
        for i in range(servo_num):
            dxl_goal_position[i] = sin_fun(phi=phi[i], t=t, T=T)

        # Write goal position
        for i in range(servo_num):
            dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID[i], ADDR_MX_GOAL_POSITION,
                                                                      dxl_goal_position[i])
            time.sleep(0.1)  # delay 0.1s---------------------------------------------------------------------------------

            if dxl_comm_result != COMM_SUCCESS:
                print("%s" % packetHandler.getTxRxResult(dxl_comm_result), "servo_num is :", i + 1)
            if dxl_error != 0:
                print("%s" % packetHandler.getRxPacketError(dxl_error), "servo_num is :", i + 1)
        judge = []
        for i in range(servo_num):
            judge.append('1')
        while True:
            # Read present position
            for i in range(servo_num):
                dxl_present_position[i], dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler,
                                                                                                  DXL_ID[i],
                                                                                                  ADDR_MX_PRESENT_POSITION)
                if dxl_comm_result != COMM_SUCCESS:
                    print("%s" % packetHandler.getTxRxResult(dxl_comm_result), "servo_num is :", i + 1)
                if dxl_error != 0:
                    print("%s" % packetHandler.getRxPacketError(dxl_error), "servo_num is :", i + 1)
                if abs(dxl_goal_position[i] - dxl_present_position[i]) < DXL_MOVING_STATUS_THRESHOLD[i]:
                    judge[i] = '0'
                else:
                    dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID[i],
                                                                              ADDR_MX_GOAL_POSITION,
                                                                              dxl_goal_position[i])
                    if dxl_comm_result != COMM_SUCCESS:
                        print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
                    elif dxl_error != 0:
                        print("%s" % packetHandler.getRxPacketError(dxl_error))
            print('t=', t)
            print('present position=', dxl_present_position)
            print('goal_position=', dxl_goal_position)
            print('judge=', judge)
            if '1' not in judge:
                break

    # Disable Dynamixel Torque
    # for i in range(servo_num):
    #     dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID[i], ADDR_MX_TORQUE_ENABLE,
    #                                                               TORQUE_DISABLE)
    #     if dxl_comm_result != COMM_SUCCESS:
    #         print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
    #     elif dxl_error != 0:
    #         print("%s" % packetHandler.getRxPacketError(dxl_error))

    # # Close port
    # portHandler.closePort()

四、成果演示

目前只实现了翻滚动作,但是核心代码基本完成,下面就是增加舵机和拟定参数,拟定参数可能要加仿真,不然非人力所为。(视频在上传…)

总结

怎么说呢,代码在编写时候咨询了别人,自己做这个学了solidworks和python,但是觉得自己学的不够深入,在应用方面很欠缺,做这个时候也咨询了很多次老师,乔老师真的是很厉害,其他的指导老师不一定会给你讲太多指导太多,但是他是真的会的,我希望通过这次项目制作能够有提升,然后就是细细规划之后的事情,学习,考研,科研比赛等等…

下图是乔老师给的指导
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

下图是组装时候拍摄的

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述