Kinect XBOX 360和六轴机械臂的实时映射

  • ROS与UR5机械臂的通讯
  • Kinect1 进行人体骨骼追踪
    • 骨骼追踪
  • Kinect得到的数据控制UR5机械臂运动
  • 总结

时间：2019-06
地点：深圳
邮箱：583259763@qq.com

=================================================================================

内容概要：
1.如何使用ROS控制UR5机械臂；
2.使用Kinect 1 进行人体骨骼追踪；
3.将提取的骨骼关节角度参数传给UR5机械臂。

环境及主要工具：
Ubuntu16.04、ROS(Kinetic)、CLion、OpenNI等等。

=================================================================================

ROS与UR5机械臂的通讯

UR5系列的机械臂支持使用ROS进行控制，相关的功能包在ROS开源社区就可以找到。所以安装相关的功能包非常的简单,使用如下的命令就可以：

sudo apt-get install ros-kinetic-universal-robot

当时控制真实UR机械臂时这个功能包会有问题，解决方法可以参照下面的链接ur机械臂实体联调解决方法，这样我们就完成了第一步。

Kinect1 进行人体骨骼追踪

首先我们说一下骨架空间，在Kinect里面，是通过20个关节点来表示一个骨架的，具体可以由图3-8看到。当你走进Kinect的视野范围的时候，Kinect就可以把你的20个关节点的位置找到（此时你得站着），位置通过（x,y,z）坐标表示。这样。你在Kinect前面做的很多复杂的动作的时候，因为人的动作和这些关节点的位置的变化由很大的关系。当电脑拿到这些数据后，对理解你做什么动作就很有帮助了。

骨骼追踪

在Ubuntu上使用的IDE是CLion，用起来很方便，Openni提取骨架信息代码如下：

#include <stdlib.h>
#include <iostream>
#include <vector>#include <XnCppWrapper.h>
#include <XnModuleCppInterface.h>
#include "cv.h"
#include "highgui.h"using namespace std;
using namespace cv;//#pragma comment (lib,"cv210")
//#pragma comment (lib,"cxcore210")
//#pragma comment (lib,"highgui210")
//#pragma comment (lib,"OpenNI")//【1】
xn::UserGenerator userGenerator;xn::DepthGenerator depthGenerator;
xn::ImageGenerator imageGenerator;/*XN_SKEL_HEAD          = 1,    XN_SKEL_NECK            = 2,XN_SKEL_TORSO         = 3,    XN_SKEL_WAIST           = 4,XN_SKEL_LEFT_COLLAR        = 5,    XN_SKEL_LEFT_SHOULDER        = 6,XN_SKEL_LEFT_ELBOW        = 7,  XN_SKEL_LEFT_WRIST          = 8,XN_SKEL_LEFT_HAND          = 9,    XN_SKEL_LEFT_FINGERTIP    =10,XN_SKEL_RIGHT_COLLAR    =11,    XN_SKEL_RIGHT_SHOULDER    =12,XN_SKEL_RIGHT_ELBOW        =13,  XN_SKEL_RIGHT_WRIST          =14,XN_SKEL_RIGHT_HAND      =15,    XN_SKEL_RIGHT_FINGERTIP    =16,XN_SKEL_LEFT_HIP          =17,    XN_SKEL_LEFT_KNEE            =18,XN_SKEL_LEFT_ANKLE        =19,  XN_SKEL_LEFT_FOOT            =20,XN_SKEL_RIGHT_HIP          =21,    XN_SKEL_RIGHT_KNEE          =22,XN_SKEL_RIGHT_ANKLE        =23,    XN_SKEL_RIGHT_FOOT          =24
*/
//a line will be drawn between start point and corresponding end point
int startSkelPoints[14]={1,2,6,6,12,17,6,7,12,13,17,18,21,22};
int endSkelPoints[14]={2,3,12,21,17,21,7,9,13,15,18,20,22,24};// callback function of user generator: new user
void XN_CALLBACK_TYPE NewUser( xn::UserGenerator& generator, XnUserID user,void* pCookie )
{
cout << "New user identified: " << user << endl;
//userGenerator.GetSkeletonCap().LoadCalibrationDataFromFile( user, "UserCalibration.txt" );
generator.GetPoseDetectionCap().StartPoseDetection("Psi", user);
}// callback function of user generator: lost user
void XN_CALLBACK_TYPE LostUser( xn::UserGenerator& generator, XnUserID user,void* pCookie )
{
cout << "User " << user << " lost" << endl;
}// callback function of skeleton: calibration start
void XN_CALLBACK_TYPE CalibrationStart( xn::SkeletonCapability& skeleton,XnUserID user,void* pCookie )
{
cout << "Calibration start for user " <<  user << endl;
}// callback function of skeleton: calibration end
void XN_CALLBACK_TYPE CalibrationEnd( xn::SkeletonCapability& skeleton,XnUserID user,XnCalibrationStatus calibrationError,void* pCookie )
{
cout << "Calibration complete for user " <<  user << ", ";
if( calibrationError==XN_CALIBRATION_STATUS_OK )
{
cout << "Success" << endl;
skeleton.StartTracking( user );
//userGenerator.GetSkeletonCap().SaveCalibrationDataToFile(user, "UserCalibration.txt" );
}
else
{
cout << "Failure" << endl;
//For the current version of OpenNI, only Psi pose is available
((xn::UserGenerator*)pCookie)->GetPoseDetectionCap().StartPoseDetection( "Psi", user );
}
}// callback function of pose detection: pose start
void XN_CALLBACK_TYPE PoseDetected( xn::PoseDetectionCapability& poseDetection,const XnChar* strPose,XnUserID user,void* pCookie)
{
cout << "Pose " << strPose << " detected for user " <<  user << endl;
((xn::UserGenerator*)pCookie)->GetSkeletonCap().RequestCalibration( user, FALSE );
poseDetection.StopPoseDetection( user );
}void clearImg(IplImage* inputimg)
{CvFont font;cvInitFont( &font, CV_FONT_VECTOR0,1, 1, 0, 3, 5);memset(inputimg->imageData,255,640*480*3);
}int main( int argc, char** argv )
{char key=0;int imgPosX=0;int imgPosY=0;// initial contextxn::Context context;context.Init();xn::ImageMetaData imageMD;IplImage* cameraImg=cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,3);cvNamedWindow("Camera",1);// map output modeXnMapOutputMode mapMode;mapMode.nXRes = 640;mapMode.nYRes = 480;mapMode.nFPS = 30;// create generatordepthGenerator.Create( context );depthGenerator.SetMapOutputMode( mapMode );imageGenerator.Create( context );userGenerator.Create( context );//【2】// Register callback functions of user generatorXnCallbackHandle userCBHandle;userGenerator.RegisterUserCallbacks( NewUser, LostUser, NULL, userCBHandle );//【3】// Register callback functions of skeleton capabilityxn::SkeletonCapability skeletonCap = userGenerator.GetSkeletonCap();skeletonCap.SetSkeletonProfile( XN_SKEL_PROFILE_ALL );XnCallbackHandle calibCBHandle;skeletonCap.RegisterToCalibrationStart( CalibrationStart,&userGenerator, calibCBHandle );skeletonCap.RegisterToCalibrationComplete( CalibrationEnd,&userGenerator, calibCBHandle );//【4】// Register callback functions of Pose Detection capabilityXnCallbackHandle poseCBHandle;userGenerator.GetPoseDetectionCap().RegisterToPoseDetected( PoseDetected,&userGenerator, poseCBHandle );// start generate datacontext.StartGeneratingAll();while( key!=27 ){context.WaitAndUpdateAll();imageGenerator.GetMetaData(imageMD);memcpy(cameraImg->imageData,imageMD.Data(),640*480*3);cvCvtColor(cameraImg,cameraImg,CV_RGB2BGR);// get usersXnUInt16 userCounts = userGenerator.GetNumberOfUsers();if( userCounts > 0 ){XnUserID* userID = new XnUserID[userCounts];userGenerator.GetUsers( userID, userCounts );for( int i = 0; i < userCounts; ++i ){//【5】// if is tracking skeletonif( skeletonCap.IsTracking( userID[i] ) ){XnPoint3D skelPointsIn[24],skelPointsOut[24];XnSkeletonJointTransformation mJointTran;for(int iter=0;iter<24;iter++){//XnSkeletonJoint from 1 to 24skeletonCap.GetSkeletonJoint( userID[i],XnSkeletonJoint(iter+1), mJointTran );skelPointsIn[iter]=mJointTran.position.position;}depthGenerator.ConvertRealWorldToProjective(24,skelPointsIn,skelPointsOut);//【6】for(int d=0;d<14;d++){CvPoint startpoint = cvPoint(skelPointsOut[startSkelPoints[d]-1].X,skelPointsOut[startSkelPoints[d]-1].Y);CvPoint endpoint = cvPoint(skelPointsOut[endSkelPoints[d]-1].X,skelPointsOut[endSkelPoints[d]-1].Y);cvCircle(cameraImg,startpoint,1.5,CV_RGB(255,0,0),6);cvCircle(cameraImg,endpoint,1.5,CV_RGB(255,0,0),6);cvLine(cameraImg,startpoint,endpoint,CV_RGB(0,255,0),2);}}}delete [] userID;}cvShowImage("Camera",cameraImg);key=cvWaitKey(20);}// stop and shutdowncvDestroyWindow("Camera");cvReleaseImage(&cameraImg);context.StopGeneratingAll();context.Shutdown();return 0;

提取骨架信息的CmakeLists文件

cmake_minimum_required(VERSION 3.12)
project(OpenNi)set(CMAKE_CXX_STANDARD 11)#list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
#set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules)
#list(APPEND CMAKE_MODULE_PATH)
#set(CMAKE_PREFIX_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules)
#find_package(OpenNI REQUIRED)
#include_directories(${OPENNI_INCLUDE_DIR})
include_directories("/usr/include/ni")find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})add_executable(OpenNi main.cpp)target_link_libraries(OpenNi ${OpenCV_LIBS} libOpenNI.so)

Kinect得到的数据控制UR5机械臂运动

利用Kinect相机提取骨架之后，我们重点考虑右手的肩关节、轴关节以及手关节三个点的三维坐标用来计算映射方式，对应的关系如下：
⑴左右挥手的角度对应机械臂的底座自由度；
⑵上臂和两肩的连线夹角对应机械臂自下而上第二个自由度；
⑶上下臂夹角对应机械臂自下而上第二个自由度；

下面是对UR5机械臂进行关节空间控制的代码：

#!/usr/bin/env python
# -*- coding: utf-8 -*-import rospy,sys
import tf
import math
import moveit_commander
from moveit_commander import MoveGroupCommander,PlanningSceneInterface
from moveit_msgs.msg import PlanningScene,ObjectColor
from geometry_msgs.msg import PoseStamped,Pose
from control_msgs.msg import GripperCommand
import numpy as npclass MoveItFKDemo:def __init__(self):moveit_commander.roscpp_initialize(sys.argv)rospy.init_node("moveit_fk_demo",anonymous=True)#加入地这个场景的开始scene=PlanningSceneInterface()self.scene_pub=rospy.Publisher("planning_scene",PlanningScene,queue_size=5)self.colors=dict()rospy.sleep(1)reference_frame="base"ground_id="ground"scene.remove_world_object(ground_id)rospy.sleep(1)ground_size=[3,3,0.02]ground_pose=PoseStamped()ground_pose.header.frame_id=reference_frameground_pose.pose.position.x=0ground_pose.pose.position.y=0ground_pose.pose.position.z=-ground_size[2]/2.0ground_pose.pose.orientation.w=1.0scene.add_box(ground_id,ground_pose,ground_size)self.setColor(ground_id,0.8,0,0,1.0)self.sendColors()#以上为加入地场景的结束arm=moveit_commander.MoveGroupCommander("manipulator")arm.set_goal_joint_tolerance(0.001)arm.set_named_target("up")arm.go()rospy.sleep(2)# arm.set_named_target("up")# arm.go()# rospy.sleep(2)listener=tf.TransformListener()rate=rospy.Rate(10.0)#三个轴方向上的单位向量x_unit=np.array([1,0,0])y_uint=np.array([0,1,0])z_unit=np.array([0,0,1])while not rospy.is_shutdown():#计算左右挥手的角度angle1，映射到机械臂的底座上try:(trans_s_n,rot_s_n)=listener.lookupTransform("neck_1","left_shoulder_1",rospy.Time(0))(trans_e_n,rot_h_n)=listener.lookupTransform("neck_1","left_elbow_1",rospy.Time(0))except (tf.LookupException,tf.ConnectivityException,tf.ExtrapolationException):continue#计算trans_e_n在y轴(绿色)上的投影projy_trans_e_n=np.array(trans_e_n).dot(y_uint)*y_uint#print(projy_trans_e_n)#计算trans_e_n在xz平面上的投影projxz_trans_e_n=trans_e_n-projy_trans_e_n#print(projxz_trans_e_n)#计算上臂在xz平面上的投影proj_upper_hand=projxz_trans_e_n-trans_s_n#计算proj_upper_hand的长度L_proj_upper_hand=np.sqrt(proj_upper_hand.dot(proj_upper_hand))#计算angle1angle1=-1.57+np.arccos(proj_upper_hand.dot(z_unit)/L_proj_upper_hand)#print(trans_s_n)print("angle1 is %f"%angle1)#计算上臂与两肩之间的夹角angle2，映射到机械臂从下到上的第二个自由度上#计算trans_e_n在z轴(蓝)上的投影projz_trans_e_n=np.array(trans_e_n).dot(z_unit)*z_unit#计算trans_e_n在xy平面上的投影projxy_trans_e_n=trans_e_n-projz_trans_e_n#计算上臂在xy平面上的投影projxy_upper_hand=projxy_trans_e_n-trans_s_n#计算projxy_upper_hang的长度L_projxy_upper_hand=np.sqrt(projxy_upper_hand.dot(projxy_upper_hand))#计算angle2angle2=-1.57+np.arccos(projxy_upper_hand.dot(y_uint)/L_projxy_upper_hand)# print("angle2 is %f"%angle2)#计算上下臂之间的夹角angle3，映射到机械臂从下到上的第三个自由度上try:#listener.waitForTransform("right_elbow_1","right_shoulder_1",rospy.Time(0),rospy.Duration(1.0))(trans_s_e,rot_s)=listener.lookupTransform("left_elbow_1","left_shoulder_1",rospy.Time(0))(trans_h_e,rot_h)=listener.lookupTransform("left_elbow_1","left_hand_1",rospy.Time(0))except (tf.LookupException,tf.ConnectivityException,tf.ExtrapolationException):continue#计算向量的向量积vector_multi_1=np.array(trans_s_e).dot(np.array(trans_h_e))#L_trans_s_e=np.sqrt(trans_s_e[0]**2+trans_s_e[1]**2+trans_s_e[2]**2)#L_trans_h_e=np.sqrt(trans_h_e[0]**2+trans_h_e[1]**2+trans_h_e[2]**2)#计算两个向量的长度L_trans_s_e=np.sqrt(np.array(trans_s_e).dot(np.array(trans_s_e)))L_trans_h_e=np.sqrt(np.array(trans_h_e).dot(np.array(trans_h_e)))angle3=math.acos(vector_multi_1/(L_trans_h_e*L_trans_s_e))angle3=-3.14+angle3# print("angle3 is %f"%angle3)#print(trans_h_e)#print("Rotation roll is %f,pitch is %f,yaw is %f"%(roll,pitch,yaw))#rate.sleep();if(angle2>-0.08):angle2=-0.08joint_positions=[angle1,angle2,angle3,-1.57,-1.57,0]arm.set_joint_value_target(joint_positions)arm.go()#rate.sleep()rospy.sleep(0.5)moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)def setColor(self,name,r,g,b,a=0.9):color=ObjectColor()color.id=namecolor.color.r=rcolor.color.g=gcolor.color.b=bcolor.color.a=aself.colors[name]=colordef sendColors(self):p=PlanningScene()p.is_diff=Truefor color in self.colors.values():p.object_colors.append(color)self.scene_pub.publish(p)if __name__=="__main__":try:MoveItFKDemo()except rospy.ROSInterruptException:pass

以下是以下效果图：



总结

利用Kinect采集人的骨架参数，实时映射到机械臂的几个自由度上面让其模仿人的手臂的动作。主要做了以下的工作：使用Kinect相机，运用一款跨平台的骨骼跟踪包openni完成了人体骨骼的追踪；将Kinect采集的信息处理后传给UR机械臂，完成模仿手臂的运动。

目前还存在一下的问题，也是下一阶段要解决的问题。在实验中也发现了很多问题，第一个问题当手臂没有动的时候，Kinect采集到手机仍在动，这样反映到机械臂上，机械臂就会发生抖动现象。第二个机械臂实际运动角度范围与手臂运动范围之间有差距，手臂运动范围角度更大。第三个问题当手臂运动之后等一会机械臂才运动存在较大的延迟。

在ROS方面还是一个新手，希望可以和大家多多交流。

Kinect XBOX 360和六轴机械臂的实时映射相关推荐

  1. 六轴机械臂DIY（三）开源项目介绍

    就这样一年半了,项目断断续续仍在进行,期间我混了个毕业,相信大家也经历了很多吧.最近上海疫情,毕业后的我哪里也去不了,只能在寝室等学校的投喂,那么正好,让我们项目继续. 本节主要完整介绍这个机械臂的开 ...

  2. 六轴机械臂下位机（arduino）+上位机（ROS+Moveit）---（一）机械臂硬件

    六轴机械臂下位机(arduino)+上位机(ROS+Moveit)---(一)机械臂硬件 机械部分 机械臂制作时的注意点!!!(坑) 零件的3D打印 控制器接线问题 机械部分 六轴机械臂在工业领域的运 ...

  3. 编写简单的六轴机械臂

    一.关键词: link 连杆 joint 连轴 orgin 原点 axis 轴 visual 视觉 geometry 几何 material 材料 collision 碰撞 inertial 惯量 f ...

  4. 六轴机械臂DIY（一）机械臂DIY总体规划

    一直想搞一个六轴机械臂玩玩,查了查网上的资料,发现这个开源项目已经较为成熟,但没有一个总体的教程.正好我可以记录一下我接下来的DIY过程,作为一个项目日记.(当然不确定项目会不会烂尾) 本项目参考gi ...

  5. Qt中动态显示六轴机械臂的STL三维模型

    Qt中动态显示六轴机械臂的STL三维模型 运动仿真 STL模型 openGL显示STL ASCII格式的STL文件 读取STL文件 openGL中显示STL模型 运动学变换 两个坑 最终效果 运动仿真 ...

  6. 桌面小六轴机械臂mechArm

    1 产品简介 mechArm Pi 270隶属于大象机器人"mechArm"系列小六轴机械臂,采用树莓派微处理器,支持ROS仿真模拟软件,是大象机器人面向创客创新和机器人产学研服务 ...

  7. 【机械】基于简化几何解法的六轴机械臂位置规划附matlab代码

    1 内容介绍 基于简化几何解法的六轴机械臂位置规划附matlab代码 2 部分代码 clc; clear; %载入数据 importfile('shuiping.mat'); theta_shuipi ...

  8. 六轴机械臂运动学算法原理及其推导过程

    网站上关于六轴机械臂piper算法的讲解有很多,但其腕点姿态的推到较为模糊,故此写一篇关于六轴机械臂piper算法的推导讲解,供有缘人参考,如果您觉得有用,可以点个赞,吾将不胜感激,若是推导过程存在错 ...

  9. MyCobot六轴机械臂的基本操作（二）

    上一讲我们做ssh和vnc的设置,有小伙伴问设置些有什么用,那么这里我先来解释一下这些功能有什么用处,首先我们可以通过ssh在我们的Windows桌面进行程序开发,然后上传到树莓派进行验证,我们也可以 ...

最新文章

  1. https://www.exploit-db.com/下载POC比较完善的代码
  2. 2017杭州·云栖大会第二天：阿里云发布了这些“黑科技”
  3. Python解决八皇后问题
  4. MATLAB粒子模拟代码注释
  5. PHP图片间隙用什么代码,如何解决CSS图片下面有间隙的问题
  6. scala与java混合编译出现的问题
  7. 什么是常驻内存式的开发模式?_“直播+”模式下的直播系统开发需要注意什么问题？...
  8. WebLogic调用WebService提示Failed to localize、Failed to create WsdlDefinitionFeature
  9. jude的一些基本用法
  10. Mysql5.7.2安装教程+下载地址（windows）
  11. 1. 概述--从零起步实现基于Html5的WEB设计器Jquery插件
  12. CnBlogs博文demo演示技巧比较：jsfiddle完胜
  13. 隆冬寒骨，风吹十一豪迈
  14. AcWing 1884. COW（前缀和）
  15. easyui获取图片路径_094 ego电商项目-2 菜单、图片上传、CRUD
  16. Google Chrome 扩展程序
  17. 牛客月赛1-(青蛙旅行)-(卡特兰数)
  18. Java抽象画--秒变绘图大师
  19. 渗透测试之Windows基础（新手必看）
  20. 条件随机场CRF的理解

热门文章

  1. 智能制造 | 如何快速自动生成站位表及自动与BOM校验？
  2. 【新手入门】MQTT 协议开发入门
  3. 正大国际期货怎么样？|召煮
  4. java内存溢出 栈溢出的原因与排查方法
  5. 如何查看mysql数据库的密码是什么_如何查看mysql数据库用户密码
  6. iOS自动化打包 Jenkins+Gitlab+Fastlane+蒲公英+钉钉
  7. 实习 week（三）
  8. 基于Leaflet实现图标旋转角度的效果(附源代码下载)
  9. 一条mysql语句是事务吗_没想到！我在简历上写了“精通MySQL”，阿里面试官跟我死磕后就给我发了高薪offer...
  10. X-Security X的安全控制