ROS based Control of a 7DoF Robot Arm for BMI
Reporter：Zhu Xiaoxiao
Date：2017.1.27

Self-introduction• I am a post-doc in the Prof. Cao Qixin’s Lab in Shanghai Jiao Tong
University (SJTU). The main research areas are machine vision,
modularity technology and robot localization.
• Researched in the Prof. Yokoyi’s Lab from 2016.7.1~2017.1.31. The
research project is an automatic Pick & Place robot arm for the BMI
system.

Global Alliance Lab (GAL)Contents
1. Introduction of Robotics Institute of SJTU and Prof. Cao’s Lab

2. Introduction of ROS

3. The development of the BMI robot arm system

Shanghai
China

31 °15'0“ N
121°35'0“ EXuhuicampusZhangjiang
campusFahua
campusShanghai

Qibao
campusLuwan
campusMinhang
campus

1896
1987
1935
2006

Zi qi dong lai
Siyuangate
The
Kaixuanoldest
gate
gate
gate

Today SJTU has 62 undergraduate programs covering 9 major
disciplines: economics, law, literature, science, engineering,
agriculture, medicine, management and arts.
By December 2015, SJTU has had 29 schools/departments, 25 directly-affiliated institutions, 13 affiliated hospitals, with 16,188 undergraduates and 20,347 postgraduates (13,841master degree candi
dates and 6,506 doctorate degree candidates)，2,134 overseas stu
dents.
Up to 2015, SJTU led the country for the 6th consecutive year interms of both the project number and the amount of money
issued by National Natural Science Foundation of China, ranked
second in sponsored research grants. The total number of SCI-cited
papers from 2005 to 2014 reaches 35,488, with 308,723 times ofcitation, ranking the second in China.

Robotics Institute of
SJTU was established in
1985(its predecessor
was Robot Laboratory
established in 1979).
One of the earliest
robotics professional
research organization in
China. The existing 30
teachers , about 60
PhD students, morethan 50 master students.

Prof. Ichiro Kato, the father of Japanese humanoid robot, is ourhonorary director of the Institute during his lifetime.

Shanghai No. 1 is the China 's first teaching-and-replay robot

Former President Jiang Zemin waswatching the performance of "Shanghai
No. 1" on the conference of major
science and technology

Former Prime Minister Zhu Rongji in
Shanghai watched "Shanghai No. 1"
Performance, on "the introduction ofdigestion Exhibition"

Researching a lot kind of robots

Fire robot

Electric cleaning
robot

Four legged robot

EOD robot

Wall climbing robot

Cable robot
12

Cooking robot

Prof. Cao’s Lab
Get the Ph.D. in Agricultural MechanicalEngineering, Kagoshima University, Japan.
And then teaching in SJTU from 1998.
He is the executive deputy director of the Instituteof Robotics and the director of the Institute of
Biomedical Manufacturing and Life Quality Engineering.
Published more than 70 SCI/EI journal papers, obtained 90
Chinese patents, and 4 Chinese academic awards.

Robots for Agricultural

Precision seedingmachineHarvest measurement
systemFruit sortor based on
visual inspection

Auto planting
systemProf. cao, guided students to participate in the NHK 1998
ROBOCON competition in Japan won the mayor Award. And heorganized the first china’s robot football competition in 2002. Since
2003, he began as the CCTV National College robot contest
evaluation expert

Service robot and special robot

Flute robot

Lunar rover

Performing robot

Toy robot

Educational robot

Oil pipeline
inspection robot

Painting robot

Service robot and special robot

Industrial robot

Medical robots
Minimally invasive surgery robot

Working assistant robot

Spinal drilling robot

Robolab.sjtu.edu.cn

Contents
1. Introduction of Robotics Institute of SJTU and Prof. Cao’s Lab

2. Introduction of ROS

3. The development of the BMI robot arm system

How to develop a robot system？

Robot

Develpe a
PR2 robot

Applications

How to develop a robot system？
Ok，I need a team.
Divide the work
Body

Drivers

Arm

Algorithms

Mobile base

System managers

Head

Database

Sensors

Applications…

…

How to develop a robot system？
I need to manage
the system
Structure clear
RobustEasy management
Extendable
Easy updating…

How to develop a robot system？
Laziness is human nature
I developed a similar arm
Reuse the design and code

ReuseHow to develop a robot system？
Divide+Manage+Reuse+…
Divide+Manage+Reuse+…
Divide+Manage+Reuse+…

Modularity
Technology…

Robot modularity technology• Define a standard interface.
• Provide the communication and management system.
• Provide the tools for creating，compiling，debugging
and management the modules.
Data Ports

Middleware
framework

Function

Function

Service Ports

Robot Control
Program

Robotic
Robotic
ModularComponent

Function

Function

Robot modularity technology
nameorginization

Used
standard

OROCOS

K.U. Leuven in Belgium , LAAS Toulouse in
France and KTH Stockholm in Sweden,
CORBA
www.orocos.org/

Orca

KTH Stockholm, orcarobotics.sourceforge.net/

Ice

ROS

Willow Garage , www.ros.org/

TCPROS

OpenRT
M-aistObject Management Group (OMG) and
AIST, www.openrtm.org

CORBA

Miro, UPnP Robot Middleware, ASEBA, Player / Stage, The PEIS
Kernel, OriN, MARIE, RSCA, The Middleware of AWARE, Sensory
Data Processing Middleware, Distributed Humanoid Robots
Middleware, Layer for Incorporation, WURDEROS(Robot Operating System)• The ROS is a flexible framework for writing robot software. It is acollection of tools, libraries, and conventions that aim to simplify thetask of creating complex and robust robot behavior across a wide
variety of robotic platforms.
• ROS is open-source and encourage collaborative robotics softwaredevelopment.

Introduction of ROS
Peer-to-peer graph network system

Robot-oriented functionality

urdf

Tf

Introduction of ROS
3D visualization

Data recording andplaying back

System monitor

CommandLine Tools

Introduction of ROS• Provide 2500+ function packages.
• Integrated great software

RobotsSensors

Introduction of ROS• ROS wiki
• ROS Answer• ROS blog
• ROSCon
Smart factory based on ROS

ROS greatly accelerating the development

Contents
1. Introduction of Robotics Institute of SJTU and Prof. Cao’s Lab

2. Introduction of ROS

3. The development of the BMI robot arm system

Background
Paralysis (麻痺) is loss of muscle function for one
or more muscles.
Reason: damage in the nervous system, such as,
stroke, trauma with nerve injury, poliomyelitis,
ALS, etc.
ALS(Amyotrophic lateral sclerosis,筋萎縮性側索硬化症 )Stephen William Hawking[wikipedia.org]

2014 ice bucket challengeBrain Machine interface (BMI)
SU: Single-unit recording
ECoG: Electrocorticography
EEG: Electroencephalography
SensorFeedback:
Visual, vocal, Tactile

SignalreadingSignal Processing
and transformationcommands
Invasion
PrecisionFrequency

low
SU

ECoG

EEG

Project objective
EEG
SensorFeedback:
Visual, vocal, Tactile

SignalreadingSignal Processing
and transformationcommands• Using EEG sensor and develop
brain signal classification
algorithm• Develop a light but powerfulrobot arm
• Add some Automaticfunctions to assistant thepatents• Main task is grasping theobjects on the table

System overview

HMI system

System overview
Task space

Real
world

SensorSensor data
Algorithm, robot controller

RGBD Sensor,
Distance sensor

VirtualVirtualworld

RGBD Sensor,
Distance sensor

Joint Angles
Robot

Object Recognition,
Grasping point
Search,System calibration,
Path planning
Robot controllers,
HW interface
Systemmonitor

VirtualBMI robot arm• Basic System• Hardware design• Vision sensor• System calibration• Simulated robot• Base robot control function• Machine vision based automatic grasping• Control sharing with BMI system

Hardware design• 各种参数，自由度展示• 控制接口，位置控制

既存の超軽量ロボットアームとの比較
項目

本研究

MICO²+KG-2

制御軸数

7

6

最大リーチ[m]0.80.7本体質量[kg]

2.6

5.2

可搬質量[kg]

1.0

0.8

ハンド動作
自由度

1
（握り・開き）2（円接線）

1（握り・開き）
0.2
（直線）

最大速度
[m/s]

本研究のアームはMICO2+KG-2と比較して，出力/重量比が2.5倍
自由度数/重量比が2.3倍

ヒトの関節に対応した自由度を有する

Lightweight and powerful
Muti metrlal
Aluminum alloy
carbon fiber
3D printer PLA

A lot of lightening hole

ロボットアーム

ワイヤ干渉駆動

Coupled Tendon-Driven(ワイヤ干渉駆動)
特願2016-109120
Two motor arecooperating at
every time.

Parallel control of the motors
最大トルク：67.0kg・cm●最高スピード：0.22s/60°
SH embedded
control system●寸法：51×32×39.5mm（突起部除く）●重量：103g（サーボホーン含む）●最大動作角度：270°●最大消費電流：6.1A●ギヤ種類：特殊アルミギヤ＋ステンレスギヤ●ケース材質：アルミ（トップ、ミドル）
ガラス入り樹脂（ボトム）●ギヤ比：362.88:1●電源電圧：HV仕様（9V～12V）●通信規格：ICS3.5（シリアル/PWM選択式）●通信速度：115200/625000/1250000bps●初期設定：ID0/BR115200●信号レベル：TTL

Repeat accuracy testing
Test Pose

Random Pose

5 measures for one Pose

• 3 Poses aretested

Pose1

Pose2

Pose3

Average(mm)

Max absolute
error
(mm)

X

20.72833

0.16544Y

47.34129

0.426108z

3.725552

0.126973

X

37.14884

0.100315Y

53.16144

0.136713z-3.60846

0.200365

X

25.38503

0.184033

Y

22.46846

0.512729z-2.37699

0.176066

Vision sensor system• RGB-D sensor can simultaneously capture the• RGB —— color information• D —— Depth information
color
CameraRight IR
CameraIR
Projector

Left IR
CameraOne frame of sensor data

Vision System calibrationqi  R  pi  t
Narg min  qi  ( R  pi  t )R, tcamera pi  , i  1, 2,...N

i1

1 N
p    pi
N i 1p  pi  p

1 N
q    qi
N i 1qi'  qi  qNH  q  p
'
i

robot

qi  , i  1, 2,...N

2

'T
i

N

arg min  qi  R  pi

'
i

2

i1

SVD Decomposition
H  U  S V Ti1||R  p  t - q ||err 
=0.021mini

R  V U Tt  q  R p

Simulate robot• Gazebo is integrated in ROS
Virtual robot

Physical simulation≈ Physical properties≈ Task space information≈ Sensor information= Control interface

Sensor data

Real robot

Robot Control

Basic Control functions

Joint Control

Trajectory Control

Cartesian Control

BMI robot arm• Basic System• Machine vision based automatic grasping• Object recognition• Grasping point determining• Arm path planning• Demonstrating• Control sharing with BMI system

Machine vision based automatic grasping• Roadmap of the grasp system.

Sensor Data

ObjectRecognition

Object Pose

Grasping Grasp Pose Robot Arm
Path Planning
Point Search

SystemCalibration

0°
30°
10°
15°
0°
0°
0°

Object recognition• Detect the objects on the table

2D ICP

Clustered point
cloud

Sensor data
RANSAC

Euclideanclustering

Recognized plane

Recognized
point cloudRecognition result• Run at 3.5Hz• The object most be a
Rotational symmetry with Z
axis• The Z axis most Perpendicular
to the table plane• Not use the color information

Using CNN for object recognition
SSD: Single Shot MultiBox Detector (Wei Liu etc.，2016)• Robust to backgroundand Motion blur
• Running at real-time
200 images for training

Machine vision based automatic grasping• Roadmap of the grasp system.

Sensor Data

ObjectRecognition

Object Pose

Grasping Grasp Pose Robot Arm
Path Planning
Point Search

SystemCalibration

0°
30°
10°
15°
0°
0°
0°

Task Space Region(TSR)(Berenson 2011)• Continuous graspable area.

o

o
eR
eT  
0

x
y
 
o
 z
et
 
1   x 
 y  
 z  xmin xmax 
y

y
min
max


z
zmax oB   min
x max  x min y min  y max  z min  z max 

0
0

0
o
B
0
0

 

0
0 
0

0
0



Grasping point searching• We use searching method to find the grasp point• Generate a lot possible grasp poses of the grasper.
• Check those pose include the pre-grasp pose with Inverse Kinematic, collisionetc.
2 ∗ 𝑐𝑜𝑠𝜃,
𝑖𝑓 𝜃 > 0
Score =• Compute a score for every possible grasp pose.
𝑐𝑜𝑠𝜃,
𝑖𝑓 𝜃 < 0

IK？
Pre-Grasp
Point
Pose generate𝜃？
Checking

Scoreing

Grasping point searching
Candidates ofgrasping pose

• This method is stable• By using parallel computing
the search speed is
acceptable• Not work well for objects withcomplex shapeDeep learning based grasping point computing• features：• 1.For different objects in high clutter grasping• 2.No human demonstration, No human labeling• 3.Suitable for different size of gripper. No retraining• 4.Better for known objects; Suitable for unknown
objects.
• 5.Can be combined with object detection
scan the scene

find the best grasps

sample the grasps

on-linepredict

sample the grasps

train CNN
2

3D object library

get the point cloud
between each grasp

whether it is a force
closure grasp

off-lineMachine vision based automatic grasping• Roadmap of the grasp system.

Sensor Data

ObjectRecognition

Object Pose

Grasping Grasp Pose Robot Arm
Path Planning
Point Search

SystemCalibration

0°
30°
10°
15°
0°
0°
0°

Robot arm path planning
Using a RRT ( rapidly exploring random tree) method to find a Collisionfree path.

End

Start

Simulation experiment

Real robot experiment

BMI robot arm• Basic System• Machine vision based automatic grasping• Control sharing with BMI system• Strategy• ExperimentSharing control with BMI system
RGB-D Camera

Start

B

• Using state switchingsuch that either the
BMI user or the
robotic system had
control during specificphases• A Finite state machine
system is used.

Init

End

Move
自律制御

(No
object)

４値制御

２値制

A

物
体

２値制御
４値制御

Place

自律制御

Grasp

Move

自律制御

自律制御
２値制御

BMI：
Robot control

自律制御:
物体認識, 障害物回避，経路・軌道計画,
把持姿勢探索，物体の把持・保持・放す

４値制御(Object)

Finite state machine

BMI grasping experiment• SSVEP is used
for BMI system• 10 tests are all
successful andthe averagegrasping timeis 1 mins

conclusion• A light but powerful robot arm is designed. The robot arm
has a similar DOF configuration and work space withhuman. The repeat precision is high.
• A vision based automatic grasping system is developed,
which has good stability for special grasping task. Moreintelligent function is under developing.
• A Finite State Machine system is used to communication
with the BMI.
• The robot control software is based on ROS.

Thank you for your attention!