Lesson 4: 3轴码垛逆解模型及实现

对于3轴码垛机械臂控制最基本的是对其建立运动学模型,而对于3轴码垛类型机械臂来说运动学模型,其本质就是给定空间3D坐标,求解3个轴的旋转角度。

 

如上图所示,左侧为实物坐标,右侧图为抽象到坐标系的几何表示,逆解过程就是知道末端坐标,而求解各个轴的旋转角度,进而转换为步进电机的步进数。上图中涉及到5个点:

O点:坐标原点,也就是机械臂底盘原点

A点:是两个电机安装位置轴心点,此点的Z轴坐标为已知的固定量

B点:两个活动臂的连接点

C点:臂末端点

D点:末端工具安装点

E点:末端工具的工作点,逆解就是知道这点的坐标,求3个马达的选择角度

下面我们利用立体几何,和解析几何知识来进行你运算分析。

一,假设条件

  • 坐标系采用右手坐标系,如上图所示
  • 机械臂的底座位于右手坐标系的XY平面
  • 底座旋转轴的位置,即为坐标系的原点位置,即上图做所视的O点
  • 我们把AB线所表示臂称之为大臂,BC线所表示的臂称之为小臂
  • 点O,A,B,C,D,E始终处于同一平面,此平面与Y轴的夹角为α
  • OA线与AB延长线之间的夹角为β
  • AB线与CB线延长线之间的夹角为γ
  • 工具安装位置CD始终与XY平面平行
  • 工具安装点与末端工作点的直线DE始终垂直于XY平面
  • 机械臂的初始位置为A,B,C三点的Y轴坐标为O,B,C点收回到距离Z轴最近的位置
  • 初始位置α=0
  • 已知条件:OA,AB,BC,CD,DE长度
  • OA与XY平面之间的夹角θ为固定值

二、数学求解

已知E点的坐标(x,y,z)基, 于以上假设条件,求解α,β,γ。

1.求解α

c点坐标为(cx,cy,cz),与E点的坐标是简单的直角坐标系对应关系

α为整个机械臂投影到XY轴平面的投影线与X轴的角度,可以直接根据E点的坐标使用正切函数计算出α

tan(α)=y/x

α=atan(y/x)

由于在机械臂的运动范围内x坐标可能为0,所以需要对x=0的坐标点做特殊的处理,详见代码

2.求解γ

γ是三角形ABC,角ABC沿AB延长线的补角,所以只要求出角ABC,即可求出γ。角ABC只需要采用三角函数即可以求得,所以求解步骤为:

2.1 求A点的坐标(ax,ay,az)

ax=OA*cos(θ)*sin(α)

ay=OA*cos(θ)*cos(α)

az=OA*sin(θ)

2.2求解三角形AC边长

AC=sqrt((cx-ax)(cx-ax)+(cy-ay)(cy-ay)+(cz-az)(cz-az))

2.3求解γ

γ=180-acos((AB*AB+ BC*BC- AC*AC) / (2 * AB* BC))

3.求解β

三角形ABC的的BAC角是可以根据三角函数求出来,γ在OA的斜率大于AC的斜率时γ等于角BAC减去直线OA和AC的夹角

3.1.求解角ABC

ABC=acos((AB*AB+ AC*AC- BC*BC) / (2 * AB* AC))

3.2.求解OA的斜率

Koa=sin(θ) / cos(θ);

3.3.求解AC的斜率

Kac= (c_y – a_y) / (c_x – a_x);

3.4.求解OA和AC的夹角

OA_AC=atan(|Koa-Kac|/(1-Koa*Kac))

3.5.求解β

如果Koa>Kac ,β=ABC-OA_AC

如果Koa<Kac ,β=ABC+OA_AC

 

4.代码实现

逆解部分的都在planner.cpp文件的plan_buffer_line函数中

 ///////////////diego_3dof kinemitacs///////////////////////
  ///alpha  oa_map_xy to y
  float alpha = 0;
  if(y==0){///////////in the x line
    if(x>0){
      alpha=PI/2;
      c_x=x-DEFAULT_CD_LENGTH_MM;
    }else if(x<0){ alpha=-PI/2; c_x=x+DEFAULT_CD_LENGTH_MM; } }else{ float tan_alpha=x/y; if(tan_alpha==0){///in the y line if(y>0){
        alpha=0;
        c_y = y - DEFAULT_CD_LENGTH_MM;
      }else{
        alpha=PI;
        c_y = y + DEFAULT_CD_LENGTH_MM;
      }
    }else if(tan_alpha>0){
      if(y>0){
        alpha=atan(tan_alpha);
      }else{
        alpha=atan(tan_alpha)-PI;
      }
      c_y=y-DEFAULT_CD_LENGTH_MM*cos(alpha);
      c_x=x-DEFAULT_CD_LENGTH_MM*sin(alpha);
    }else{
      if(y>0){
        alpha=atan(tan_alpha);
      }else{
        alpha=PI/2-atan(tan_alpha);
      }
      c_y=y-DEFAULT_CD_LENGTH_MM*cos(alpha);
      c_x=x-DEFAULT_CD_LENGTH_MM*sin(alpha);
    }
  }

  Serial.print("------------alpha\r\n");
  Serial.print(alpha*180/PI);
  Serial.print("\r\n");
  //gamma 180- ab to bc
  float gamma = 0;
  float default_oa = DEFAULT_OA_LENGTH_MM;
  float default_ab = DEFAULT_AB_LENGTH_MM;
  float default_bc = DEFAULT_BC_LENGTH_MM;
  float default_theta = DEFAULT_THETA_DEGREE;

  float a_x = default_oa * cos(default_theta) * sin(alpha);
  float a_y = default_oa * cos(default_theta) * cos(alpha);
  float a_z = default_oa * sin(default_theta);

  float ac = sqrt(pow(c_x - a_x, 2) + pow(c_y - a_y, 2) + pow(c_z - a_z, 2));

  float cos_gamma = (pow(default_ab, 2) + pow(default_bc, 2) - pow(ac, 2)) / (2 * default_ab * default_bc);
//  Serial.print("-------------cos_gamma");
//  Serial.print(cos_gamma);

  
  //gamma = PI - acos(cos_gamma);
  gamma = acos(cos_gamma);

  Serial.print("-------------gamma\r\n");
  Serial.print(gamma*180/PI);
  Serial.print("\r\n");

  ///beta; map to the axis between oa to ab
  float beta = 0;
  float k_map_oa = sin(default_theta) / cos(default_theta);

  float a_map_x = default_oa * cos(default_theta);  ////62.47
  float a_map_y = default_oa * sin(default_theta);  ////101.7

  float c_map_x = sqrt(pow(c_x, 2) + pow(c_y, 2)); ////130
  float c_map_y = c_z; ////102
  float k_map_ac = (c_map_y - a_map_y) / (c_map_x - a_map_x);
//  Serial.print("-------------c_map_y\r\n");
//  Serial.print(c_map_y);
//  Serial.print("\r\n");
//  Serial.print("-------------a_map_y\r\n");
//  Serial.print(a_map_y);
//  Serial.print("\r\n");
//  Serial.print("-------------c_map_x\r\n");
//  Serial.print(c_map_x);
//  Serial.print("\r\n");
//  Serial.print("-------------a_map_x\r\n");
//  Serial.print(a_map_x);
//  Serial.print("\r\n");
//  Serial.print("-------------k_map_oa\r\n");
//  Serial.print(k_map_oa);
//  Serial.print("\r\n");
//  Serial.print("-------------k_map_ac\r\n");
//  Serial.print(k_map_ac);
//  Serial.print("\r\n");

  float tan_oa_ac = abs(k_map_oa - k_map_ac) / abs(1 - k_map_oa * k_map_ac);
  float cos_ab_ac = (pow(default_ab, 2) + pow(ac, 2) - pow(default_bc, 2)) / (2 * default_ab*ac);
  float ab_ac = acos(cos_ab_ac);
//  Serial.print("-------------ab_ac\r\n");
//  Serial.print(cos_ab_ac);
//  Serial.print("\r\n");
//  Serial.print(ab_ac);
//  Serial.print("\r\n");
  float ac_oa = atan(tan_oa_ac);
//  Serial.print("-------------ac_oa\r\n");
//  Serial.print(ac_oa);
//  Serial.print("\r\n");
  
  if (ac_oa < ab_ac) { if (k_map_ac > k_map_oa) {
      beta = 0 - ab_ac - atan(tan_oa_ac);
    } else {
      beta = 0 - ab_ac + atan(tan_oa_ac);
    }
  } else {
    float oc = sqrt(pow(c_map_x, 2) + pow(c_map_y, 2));
    float cos_oa_ac = (pow(default_oa, 2) + pow(ac, 2) - pow(oc, 2)) / (2 * default_oa * ac);
    float oa_ac = acos(cos_oa_ac);
    beta = PI - oa_ac - ab_ac;
  }

  Serial.print("-------------beta\r\n");
  Serial.print(beta*180/PI);
  Serial.print("\r\n");

  float degree_per_step_axis1 = 2*PI / (DEFAULT_STEPS_PER_CYCLE_AXIS_1 * DEFAULT_REDUCTION_RATE_AXIS_1 * DEFAULT_MICROSTEPS_AXIS_1); //steps per cycle
  float degree_per_step_axis2 = 2*PI / (DEFAULT_STEPS_PER_CYCLE_AXIS_2 * DEFAULT_REDUCTION_RATE_AXIS_2 * DEFAULT_MICROSTEPS_AXIS_2); //steps per cycle
  float degree_per_step_axis3 = 2*PI / (DEFAULT_STEPS_PER_CYCLE_AXIS_3 * DEFAULT_REDUCTION_RATE_AXIS_3 * DEFAULT_MICROSTEPS_AXIS_3); //steps per cycle

  float axis1_degree_steps = alpha / degree_per_step_axis1;
  float axis2_degree_steps = beta / degree_per_step_axis2;
  float axis3_degree_steps = gamma / degree_per_step_axis3;

  target[X_AXIS] = (int32_t)(axis1_degree_steps + 0.5);
  target[Y_AXIS] = (int32_t)(axis2_degree_steps + 0.5);
  target[Z_AXIS] = (int32_t)(axis3_degree_steps + 0.5); 

全部代码请见gitclub

Lesson 2: 硬件连接

一、硬件连接

Diego 3dof的硬件连接比较简单,Arduino和A4988是通过Arduino CNC sheild v3连接的,Arduino CNC sheild可以直接插在Arduino上,然后再把3个A4988插在Arduino CNC sheild上即可,连接效果如下:

CNC sheild可以支持4路步进电机,由于我们3轴码垛只使用到了3个步进电机,所以只需要插3个A4988即可,步进电机去A4988的对应关系如下图:

 

二、Arduino Pin对应关系

A4988及马达编号Arduino IO对应功能
1D2马达1步进脉冲
1D5马达1方向
2D3马达2步进脉冲
2D6马达2方向
3D4马达2步进脉冲
3D7马达2方向

 

 

三、其他

1.Diego 3轴机械臂的机械部分需要根据图纸进行钣金加工,或者也可以3D打印,钣金加工的精度要高与3D打印的精度

2. 安装过程中需要用到的不同规格的螺丝刀,电钻等工。

Lesson 21:moveit-diego1# plus 的moveit驱动

在上节课中我们已经通过moveit assistant完成了针对diego 1# plus的配置,但如果想让机械臂动起来,我们还需要对编写驱动代码:代码原理如下:

首先,moveit把计算的结果通过Ros action的方式发送给driver,driver调用Ros_arduino_bridge的servor_write server发送各个关节舵机的控制指令给Arduino uno控制板
其次,同时Driver也要通过调用Ros_arduino_bridge的servo_read服务读取各个关节的舵机状态,通过joint_state消息的方式发送给moveit,供moveit进行路径规划计算。

接下来我们一步一步的来完成驱动

1.创建控制器配置文件diego_controllers.yaml

在moveit assistant产生的配置文件目录的config子目录下,创建Diego 1# plus的配置文件diego1_controllers.yaml,此文件告诉moveit将与哪个的action通讯

diego1_controllers.yaml代码如下:

controller_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - arm_base_to_arm_round_joint_stevo0
      - shoulder_2_to_arm_joint_stevo1
      - big_arm_round_to_joint_stevo2
      - arm_joint_stevo2_to_arm_joint_stevo3
      - wrist_to_arm_joint_stevo4
      - arm_joint_stevo4_to_arm_joint_stevo5
  - name: gripper_controller
    action_ns: gripper_action
    type: GripperCommand
    default: true
    joints:
      - hand_to_grip_joint_stevo6
      - hand_to_grip_joint_stevo7

这里分别为机械臂和机械爪定义了两个控制器arm_controller和gripper_controller,告诉moveit机械臂通讯的topic是arm_controller/follow_joint_trajectory,机械爪通讯的topic是gripper_controller/gripper_action

2.机械臂控制的FollowController

我们扩展了ros_arduino_bridge的功能来实现FollowController,在ros_arduino_python目录下创建joints.py和diego1_follow_controller.py

2.1 joint.py

封装了关节舵机控制的代码,主要功能函数:

  • __init__类初始化代码
  • setCurrentPosition(self):通过调用/arduino/servo_write服务发送舵机的控制命令,把舵机的位置设置为self.position
  • setCurrentPosition(self, position):
    通过调用/arduino/servo_write服务发送舵机的控制命令,把舵机的位置设置为moveit给出的舵机位置
  • mapToServoPosition(self,position):将moveit给出的舵机位置,转换为机械臂舵机实际对应的位置,此功能需要标定后,才能准确控制机械臂的位置。
#!/usr/bin/env python

# Copyright (c) 2017-2018 diego. 
# All right reserved.
#

## @file joints.py the jonit clase support functions for joints.

## @brief Joints hold current values.
import roslib
import rospy
from ros_arduino_msgs.srv import *
from math import pi as PI, degrees, radians
class Joint:

    ## @brief Constructs a Joint instance.
    ##
    ## @param servoNum The servo id for this joint.
    ## 
    ## @param name The joint name.
    ## 
    ## @param name The servo control range.
    def __init__(self, name, servoNum, max, min, servo_max, servo_min, inverse):
        self.name = name
        self.servoNum=servoNum
        self.max=max
        self.min=min
        self.servo_max=servo_max
        self.servo_min=servo_min
        self.inverse=inverse

        self.position = 0.0
        self.velocity = 0.0

    ## @brief Set the current position.
    def setCurrentPosition(self):
        rospy.wait_for_service('/arduino/servo_write')
	try:
	        servo_write=rospy.ServiceProxy('/arduino/servo_write',ServoWrite)
	        servo_write(self.servoNum,self.position)
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e   

    ## @brief Set the current position.
    ##
    ## @param position The current position. 
    def setCurrentPosition(self, position):
        rospy.wait_for_service('/arduino/servo_write')
	try:
		#if self.servoNum==2:
        	servo_write=rospy.ServiceProxy('/arduino/servo_write',ServoWrite)
        	self.mapToServoPosition(position)	        	
       		servo_write(self.servoNum,radians(self.position))

        except rospy.ServiceException, e:
            print "Service call failed: %s"%e
    
    ## @brief map to  Servo Position.
    ##
    ## @param position The current position.        
    def mapToServoPosition(self,position):
	    per=(position-self.min)/(self.max-self.min)
	    if not self.inverse:	    	
	    	self.position=self.servo_min+per*(self.servo_max-self.servo_min)
	    else:
	    	self.position=self.servo_max-per*(self.servo_max-self.servo_min)


2.2diego1_follow_controller.py

此类diego机械臂控制器类,接收moveit的控制命令,转换为关节舵机的实际控制指令,驱动机械臂的运动。

#!/usr/bin/env python

# Copyright (c) 2017-2018 williamar. 
# All right reserved.

import rospy, actionlib

from control_msgs.msg import FollowJointTrajectoryAction
from trajectory_msgs.msg import JointTrajectory
from diagnostic_msgs.msg import *
from math import pi as PI, degrees, radians
from joints import Joint

class FollowController:

    def __init__(self, name):
        self.name = name
        
        rospy.init_node(self.name)
        

        # rates
        self.rate = 10.0
        
        # Arm jonits
        self.arm_base_to_arm_round_joint_stevo0=Joint('arm_base_to_arm_round_joint_stevo0',0,1.5797,-1.5707,130,0,False)
        self.shoulder_2_to_arm_joint_stevo1=Joint('shoulder_2_to_arm_joint_stevo1',1,1.5707,-0.1899,115,45,False)
        self.big_arm_round_to_joint_stevo2=Joint('big_arm_round_to_joint_stevo2',2,2.5891,1,100,20,False)
        self.arm_joint_stevo2_to_arm_joint_stevo3=Joint('arm_joint_stevo2_to_arm_joint_stevo3',3,1.5707,-1.5707,130,0,False)
        self.wrist_to_arm_joint_stevo4=Joint('wrist_to_arm_joint_stevo4',4,1.5707,-1.5707,130,0,False)
        self.arm_joint_stevo4_to_arm_joint_stevo5=Joint('arm_joint_stevo4_to_arm_joint_stevo5',5,1.5707,-1.5707,130,0,True)
        
        
        
        self.joints=[self.arm_base_to_arm_round_joint_stevo0,
        self.shoulder_2_to_arm_joint_stevo1,
        self.big_arm_round_to_joint_stevo2,
        self.arm_joint_stevo2_to_arm_joint_stevo3,
        self.wrist_to_arm_joint_stevo4,
        self.arm_joint_stevo4_to_arm_joint_stevo5]
        
        # action server
        self.server = actionlib.SimpleActionServer('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction, execute_cb=self.actionCb, auto_start=False)
        self.server.start()
        rospy.spin()
        rospy.loginfo("Started FollowController")


    def actionCb(self, goal):
	print("****************actionCb*********************")    
        rospy.loginfo(self.name + ": Action goal recieved.")
        traj = goal.trajectory

        if set(self.joints) != set(traj.joint_names):
            for j in self.joints:
                if j.name not in traj.joint_names:
                    msg = "Trajectory joint names does not match action controlled joints." + str(traj.joint_names)
                    rospy.logerr(msg)
                    self.server.set_aborted(text=msg)
                    return
            rospy.logwarn("Extra joints in trajectory")

        if not traj.points:
            msg = "Trajectory empy."
            rospy.logerr(msg)
            self.server.set_aborted(text=msg)
            return

        try:
            indexes = [traj.joint_names.index(joint.name) for joint in self.joints]
        except ValueError as val:
            msg = "Trajectory invalid."
            rospy.logerr(msg)
            self.server.set_aborted(text=msg)
            return

        if self.executeTrajectory(traj):   
            self.server.set_succeeded()
        else:
            self.server.set_aborted(text="Execution failed.")

        rospy.loginfo(self.name + ": Done.")     

    def executeTrajectory(self, traj):
        rospy.loginfo("Executing trajectory")
        rospy.logdebug(traj)
        # carry out trajectory
        try:
            indexes = [traj.joint_names.index(joint.name) for joint in self.joints]
        except ValueError as val:
            rospy.logerr("Invalid joint in trajectory.")
            return False

        # get starting timestamp, MoveIt uses 0, need to fill in
        start = traj.header.stamp
        if start.secs == 0 and start.nsecs == 0:
            start = rospy.Time.now()

        r = rospy.Rate(self.rate)
	for point in traj.points:            
            desired = [ point.positions[k] for k in indexes ]
            for i in indexes:
                #self.joints[i].position=desired[i]
                self.joints[i].setCurrentPosition(desired[i])

            while rospy.Time.now() + rospy.Duration(0.01) < start:
                rospy.sleep(0.01)
        return True
    

if __name__=='__main__':
    try:
        rospy.loginfo("start followController...")
        FollowController('follow_Controller')
    except rospy.ROSInterruptException:
        rospy.loginfo('Failed to start followController...')
        


此类是驱动的核心代码,接下来我们详细解释其功能逻辑

    def __init__(self, name):
        self.name = name
        
        rospy.init_node(self.name)
        

        # rates
        self.rate = 10.0

初始化化ros note,设置节点名称,刷新频率为10hz

        # Arm jonits
        self.arm_base_to_arm_round_joint_stevo0=Joint('arm_base_to_arm_round_joint_stevo0',0,1.5797,-1.5707,130,0,False)
        self.shoulder_2_to_arm_joint_stevo1=Joint('shoulder_2_to_arm_joint_stevo1',1,1.5707,-0.1899,115,45,False)
        self.big_arm_round_to_joint_stevo2=Joint('big_arm_round_to_joint_stevo2',2,2.5891,1,100,20,False)
        self.arm_joint_stevo2_to_arm_joint_stevo3=Joint('arm_joint_stevo2_to_arm_joint_stevo3',3,1.5707,-1.5707,130,0,False)
        self.wrist_to_arm_joint_stevo4=Joint('wrist_to_arm_joint_stevo4',4,1.5707,-1.5707,130,0,False)
        self.arm_joint_stevo4_to_arm_joint_stevo5=Joint('arm_joint_stevo4_to_arm_joint_stevo5',5,1.5707,-1.5707,130,0,True)
        
        
        
        self.joints=[self.arm_base_to_arm_round_joint_stevo0,
        self.shoulder_2_to_arm_joint_stevo1,
        self.big_arm_round_to_joint_stevo2,
        self.arm_joint_stevo2_to_arm_joint_stevo3,
        self.wrist_to_arm_joint_stevo4,
        self.arm_joint_stevo4_to_arm_joint_stevo5]

初始化机械臂的关节,并把关节放入joints列表中,方便后续操作

        # action server
        self.server = actionlib.SimpleActionServer('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction, execute_cb=self.actionCb, auto_start=False)
        self.server.start()
        rospy.spin()
        rospy.loginfo("Started FollowController")

定义action server并启动。
指定通讯的topic为arm_controller/follow_joint_trajectory,类型为FollowJointTrajectoryAction,收到消息后的回调函数为self.actionCb,
self.actionCb的其实就是follow_controller对JointTrajectory msg的处理,所以这里先介绍一下JointTrajectory msg,只要理解了JointTrajectory msg,有助与后续代码的理解,执行如下命令就可以显示了JointTrajectory msg的结构。

$ rosmsg show JointTrajectory

可以看到消息的结构体中包含了三部分

  •  header  这是Ros的标准消息头这里就不多介绍了
  •  joint_names 这是所有关节名称的数组
  • JointTrajectoryPoint 这部分是驱动的关键,这个数组记录了机械臂从一种姿势到另外一种姿势所经过的路径点,moveit所产生的姿势路径是通过这些point点描述出来的,也就是我们驱动中要控制每个关节的舵机都按照这些point点进行运动,每个point又是由一个结构体构成:
  • positions这是一个float64的数组,记录每个point的时候舵机应该到达的角度,这里是弧度为单位的,比如说是6自由度的那每个Point的这个positions字段中应该包含六个数值[1.57,0,2,0.2,3,0.12],也就是我们舵机控制范围是180度,那这里面的取值范围就是0~π
  • velocities这个数组记录了每个关节运动的速度
  • accelerations这个数组记录每个关节运动的加速度
  • effort这个参数可以不用
  • time_from_start这个参数是指定从头部的timestamp开始算起多长时间要达到这个点的位置

理解了JointTrajectory msg的结构,我们很容易理解函数self.actionCb就是对消息做逻辑上的异常处理,判断msg中joint是否在joints列表中,消息是否为空,而真正消息体的处理是在executeTrajectory(self, traj),下面我们来看这个函数的处理逻辑

    def executeTrajectory(self, traj):
        rospy.loginfo("Executing trajectory")
        rospy.logdebug(traj)
        # carry out trajectory
        try:
            indexes = [traj.joint_names.index(joint.name) for joint in self.joints]
        except ValueError as val:
            rospy.logerr("Invalid joint in trajectory.")
            return False

将JointTrajectory msg中的Joint按照self.joints的顺序索引,并将索引放入indexes数组中。

    # get starting timestamp, MoveIt uses 0, need to fill in
        start = traj.header.stamp
        if start.secs == 0 and start.nsecs == 0:
            start = rospy.Time.now()

获取当前msg的时间戳

r = rospy.Rate(self.rate)

设定刷新频率

	for point in traj.points:            
            desired = [ point.positions[k] for k in indexes ]
            for i in indexes:
                #self.joints[i].position=desired[i]
                self.joints[i].setCurrentPosition(desired[i])

            while rospy.Time.now() + rospy.Duration(0.01) < start:
                rospy.sleep(0.01)

循环读取msg中的位置点,位置点中的舵机位置值读取出来,调用Joint类的setCurrentPosition方法发送舵机的控制指令

3. launch启动文件

moveit assistant会生成一个launch文件夹,其中包含了主要的launch文件,我们只需要做些接单的修改,即可进行测试。

3.1修改diego1_moveit_controller_manager.launch.xml

此文件是moveit assistant自动生成的,但其中内容是空的,我增加如下内容,告诉moveit,启动Controller的配置文件位置

<launch>
     <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
     <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
     <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
     <!-- load controller_list -->
     <rosparam file="$(find diego_moveit_config)/config/diego1_controllers.yaml"/>
</launch>

3.2创建diego_demo.launch文件

在moveit的launch文件夹下创建diego_demo.launch文件,并添加如下内容

<launch>

     <master auto="start"/>

     <node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
         <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
     </node>
     <!-- By default, we are not in debug mode -->
     <arg name="debug" default="false" />

     <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
     <include file="$(find diego_moveit_config)/launch/planning_context.launch">
         <arg name="load_robot_description" value="true"/>
     </include>

     <node name="arduino_follow_controller" pkg="ros_arduino_python" type="diego1_follow_controller.py" output="screen">
     </node>

     <!-- If needed, broadcast static tf for robot root -->
     <node pkg="tf" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world_frame base_link 100" />

     <!-- We do not have a robot connected, so publish fake joint states -->
     <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
        <param name="/use_gui" value="false"/>
        <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
     </node>

     <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
     <include file="$(find diego_moveit_config)/launch/move_group.launch">
     </include>
</launch>

这里主要是作为一个测试使用,所以joint_states使用了假数据,在调用joint_state_publisher时我们把source list设置为/move_group/fake_controller_joint_states

现在我们可以执行如下代码来启动moveit

roslaunch diego_moveit_config diego_demo.launch

这时候我们可以通过控制台输入moveit命令来控制机械臂,我们也可以用代码来调用moveit接口来控制机械臂,下面我们来写一段测试代码来

4.moveit控制测试代码

在ros_arduino_python目录下创建diego_moveit_test.py,代码如下

#!/usr/bin/env python

import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand

class MoveItDemo:
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
 
        # Connect to the arm move group
        arm = moveit_commander.MoveGroupCommander('arm')
                       
        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()
        
        # Display the name of the end_effector link
        rospy.loginfo("The end effector link is: " + str(end_effector_link))
        
        # Set a small tolerance on joint angles
        arm.set_goal_joint_tolerance(0.001)
        
        # Start the arm target in "resting" pose stored in the SRDF file
        arm.set_named_target('arm_default_pose')
        
        # Plan a trajectory to the goal configuration
        traj = arm.plan()
         
        # Execute the planned trajectory
        arm.execute(traj)
        
        # Pause for a moment
        rospy.sleep(1)         
        
        # Cleanly shut down MoveIt
        moveit_commander.roscpp_shutdown()
        
        # Exit the script
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItDemo()
    except rospy.ROSInterruptException:
        pass

在测试代码中,我们连接到group arm,并命令机械臂运行到姿势 arm_default_pose,我们运行如下命令启动测试代码

    rosrun ros_arduino_python diego_moveit_test.py

启动后可以看到机械臂马上响应到arm_default_pose的位置。
 

Scroll to top