KUKA iiwa — SmartServo随动

　　机器人控制器中的代码SmartServoJointTest.java如下。KUKA iiwa伺服随动的核心是调用setDestination(...)函数更新目标位置，参数可以是关节角度JointPosition或末端位置姿态Frame。

  1 package application;
  2 
  3 import java.io.IOException;
  4 import java.net.DatagramPacket;
  5 import java.net.DatagramSocket;
  6 import java.net.InetSocketAddress;
  7 import java.net.SocketTimeoutException;
  8 import javax.inject.Inject;
  9 import static com.kuka.roboticsAPI.motionModel.BasicMotions.*;
 10 import com.kuka.common.StatisticTimer;
 11 import com.kuka.common.StatisticTimer.OneTimeStep;
 12 import com.kuka.common.ThreadUtil;
 13 import com.kuka.connectivity.motionModel.smartServo.ISmartServoRuntime;
 14 import com.kuka.connectivity.motionModel.smartServo.ServoMotion;
 15 import com.kuka.connectivity.motionModel.smartServo.SmartServo;
 16 import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
 17 import com.kuka.roboticsAPI.deviceModel.JointPosition;
 18 import com.kuka.roboticsAPI.deviceModel.LBR;
 19 
 20 
 21 
 22 public class SmartServoJointTest extends RoboticsAPIApplication {
 23         @Inject
 24         private LBR _lbr;
 25         
 26         private JointPosition _jointPosition = new JointPosition(0,0,0,0,0,0,0);
 27         
 28         private ISmartServoRuntime _theSmartServoRuntime = null;
 29         
 30         private DatagramSocket _socket;
 31         private static final int PORT = 30003;
 32         private static final int SOCKET_TIMEOUT = 20;
 33         private static final int UDP_PACKET_SIZE = 508;
 34         private boolean isLoop = true;
 35         
 36         @Override
 37         public void initialize()
 38         {
 39             initUDP();
 40         }
 41 
 42         
 43         /**
 44          * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free.
 45          */
 46         public void moveToInitialPosition()
 47         {
 48             _lbr.move(ptpHome().setJointVelocityRel(0.1));
 49             try
 50             {
 51                 if (!ServoMotion.validateForImpedanceMode(_lbr))
 52                 {
 53                     getLogger().info("Validation of torque model failed - correct your mass property settings");
 54                     getLogger().info("SmartServo will be available for position controlled mode only, until validation is performed");
 55                 }
 56             }
 57             catch (IllegalStateException e)
 58             {
 59                 getLogger().info("Omitting validation failure for this sample
"
 60                         + e.getMessage());
 61             }
 62         }
 63 
 64         
 65         @Override
 66         public void run()
 67         {
 68             getLogger().info("Move to initial position");
 69             moveToInitialPosition();
 70 
 71             JointPosition initialPosition = new JointPosition(_lbr.getCurrentJointPosition());
 72             
 73             SmartServo aSmartServoMotion = new SmartServo(initialPosition);
 74 
 75             // Set the motion properties to 20% of systems abilities
 76             aSmartServoMotion.setJointAccelerationRel(0.2);
 77             aSmartServoMotion.setJointVelocityRel(0.2);
 78 
 79             aSmartServoMotion.setMinimumTrajectoryExecutionTime(20e-3);
 80 
 81             getLogger().info("Starting SmartServo motion in position control mode");
 82             _lbr.moveAsync(aSmartServoMotion);
 83 
 84             _theSmartServoRuntime = aSmartServoMotion.getRuntime();
 85 
 86             getLogger().info("Start SmartServo");
 87             // time measurement...
 88             StatisticTimer timing = new StatisticTimer();
 89             
 90             try
 91             {
 92                 while (isLoop)
 93                 {
 94                     // Timing - draw one step
 95                     OneTimeStep aStep = timing.newTimeStep();
 96 
 97                     _theSmartServoRuntime.updateWithRealtimeSystem();
 98                     
 99                      receiveNewPosition();
100                     
101                     // set target joint position
102                     _theSmartServoRuntime.setDestination(_jointPosition);
103 
104                     aStep.end();
105 
106                 }
107             }
108             catch (Exception e)
109             {
110                 getLogger().info(e.getLocalizedMessage());
111                 e.printStackTrace();
112             }
113             
114           
115             ThreadUtil.milliSleep(1000);
116 
117             //Print statistics and parameters of the motion
118             getLogger().info("Displaying final states after loop ");
119             getLogger().info(getClass().getName() + _theSmartServoRuntime.toString());
120             
121             // Stop the motion
122             getLogger().info("Stop the SmartServo motion");
123             _theSmartServoRuntime.stopMotion();
124 
125             getLogger().info("Statistic Timing of Overall Loop " + timing);
126             if (timing.getMeanTimeMillis() > 150)
127             {
128                 getLogger().info("Statistic Timing is unexpected slow, you should try to optimize TCP/IP Transfer");
129             }
130         }
131         
132         
133 
134         
135         /**
136          * Initialize socket for UDP communication to an external client.
137          * 
138          * @return True, if socket opened successfully. False, otherwise.
139          */
140         public boolean initUDP()
141         {
142             try
143             {
144                 _socket = new DatagramSocket(null);
145                 _socket.setReuseAddress(true);
146                 _socket.bind(new InetSocketAddress(PORT));
147                 _socket.setSoTimeout(SOCKET_TIMEOUT);
148             }
149             catch (Exception e)
150             {
151                 getLogger().error("Problems initializing UDP socket: ", e);
152                 return false;
153             }
154 
155             return true;
156         }
157 
158         
159         public void receiveNewPosition() throws IOException
160         {
161             byte[] receivedData = new byte[UDP_PACKET_SIZE];
162             DatagramPacket receivedPacket = new DatagramPacket(receivedData, receivedData.length);
163 
164             try
165             {
166                 _socket.receive(receivedPacket);
167       
168                 // parse received DatagramPacket
169                 String s = new String(receivedPacket.getData(), 0, receivedPacket.getLength());
170                 if(!s.equals("StopServo"))
171                 {
172                     String[] sList = s.split(",");
173                     for(int i = 0; i < 7; i++)
174                         _jointPosition.set(i, Double.valueOf(sList[i]));
175                 }
176                 else
177                     isLoop = false;
178                     
179             }
180             catch (SocketTimeoutException e)
181             {
182             }
183         }
184         
185         
186         /**
187          * Call this if the instance of this class is not needed anymore.
188          */
189         public void dispose()
190         {
191             if (_socket != null)
192             {
193                 _socket.close();
194             }
195         }
196         
197          
198         /**
199          * Main routine, which starts the application.
200          * 
201          * @param args
202          *            arguments
203          */
204         public static void main(String[] args)
205         {
206             SmartServoJointTest app = new SmartServoJointTest();
207             app.runApplication();
208         }
209 }

View Code

　　在VREP中控制虚拟的KUKA iiwa机器人运动，同时通过UDP将虚拟机器人的关节角度发送给机器人控制器。控制机械臂运动的脚本如下：

function sysCall_threadmain()
    jointHandles={-1,-1,-1,-1,-1,-1,-1}
    for i=1,7,1 do
        jointHandles[i]=sim.getObjectHandle('LBR_iiwa_7_R800_joint'..i)
    end

    -- Set-up some of the RML vectors:
    vel=10
    accel=40
    jerk=80
    currentVel={0,0,0,0,0,0,0}
    currentAccel={0,0,0,0,0,0,0}
    maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
    maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
    maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
    targetVel={0,0,0,0,0,0,0}

    targetPos1={0,30*math.pi/180,0,-60*math.pi/180,0,90*math.pi/180,0}
    sim.rmlMoveToJointPositions(jointHandles,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,targetPos1,targetVel)

    targetPos2={0,25*math.pi/180,0,90*math.pi/180,0,0,0}
    sim.rmlMoveToJointPositions(jointHandles,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,targetPos2,targetVel)

    sim.stopSimulation() 
end

　　发送关节位置的脚本如下：

function sysCall_init()
    -- do some initialization here:
    local socket = require 'socket'
    udp = assert(socket.udp())
    -- connect socket to the server's address and port
    local address, port = "192.168.1.100", 30003
    udp:setpeername(address, port)
    udp:settimeout(-1)

    jointPositions = {0,0,0,0,0,0,0}
    jointHandles = {-1,-1,-1,-1,-1,-1,-1}
    for i=1,7,1 do
        jointHandles[i] = sim.getObjectHandle('LBR_iiwa_7_R800_joint'..i)
    end
end


function sysCall_actuation()
    for i=1,7,1 do
        jointPositions[i] = sim.getJointPosition(jointHandles[i])
    end
    local data = string.format("%f,%f,%f,%f,%f,%f,%f",jointPositions[1],jointPositions[2],jointPositions[3],
                                    jointPositions[4],jointPositions[5],jointPositions[6],jointPositions[7])
    udp:send(data)
end


function sysCall_cleanup()
    udp:send("StopServo")
end

　　设置VREP仿真为Real-time模式开始仿真，实际的机械臂会接收虚拟机械臂发送的关节位置进行随动

　　使用SmartServoLIN可以控制机械臂沿直线随动：