KUKA iiwa — SmartServo随动

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

package application;

import java.io.IOException;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetSocketAddress;
import java.net.SocketTimeoutException;
import javax.inject.Inject;
import static com.kuka.roboticsAPI.motionModel.BasicMotions.*;
import com.kuka.common.StatisticTimer;
import com.kuka.common.StatisticTimer.OneTimeStep;
import com.kuka.common.ThreadUtil;
import com.kuka.connectivity.motionModel.smartServo.ISmartServoRuntime;
import com.kuka.connectivity.motionModel.smartServo.ServoMotion;
import com.kuka.connectivity.motionModel.smartServo.SmartServo;
import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
import com.kuka.roboticsAPI.deviceModel.JointPosition;
import com.kuka.roboticsAPI.deviceModel.LBR;



public class SmartServoJointTest extends RoboticsAPIApplication {
        @Inject
        private LBR _lbr;
        
        private JointPosition _jointPosition = new JointPosition(0,0,0,0,0,0,0);
        
        private ISmartServoRuntime _theSmartServoRuntime = null;
        
        private DatagramSocket _socket;
        private static final int PORT = 30003;
        private static final int SOCKET_TIMEOUT = 20;
        private static final int UDP_PACKET_SIZE = 508;
        private boolean isLoop = true;
        
        @Override
        public void initialize()
        {
            initUDP();
        }

        
        /**
         * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free.
         */
        public void moveToInitialPosition()
        {
            _lbr.move(ptpHome().setJointVelocityRel(0.1));
            try
            {
                if (!ServoMotion.validateForImpedanceMode(_lbr))
                {
                    getLogger().info("Validation of torque model failed - correct your mass property settings");
                    getLogger().info("SmartServo will be available for position controlled mode only, until validation is performed");
                }
            }
            catch (IllegalStateException e)
            {
                getLogger().info("Omitting validation failure for this sample
"
                        + e.getMessage());
            }
        }

        
        @Override
        public void run()
        {
            getLogger().info("Move to initial position");
            moveToInitialPosition();

            JointPosition initialPosition = new JointPosition(_lbr.getCurrentJointPosition());
            
            SmartServo aSmartServoMotion = new SmartServo(initialPosition);

            // Set the motion properties to 20% of systems abilities
            aSmartServoMotion.setJointAccelerationRel(0.2);
            aSmartServoMotion.setJointVelocityRel(0.2);

            aSmartServoMotion.setMinimumTrajectoryExecutionTime(20e-3);

            getLogger().info("Starting SmartServo motion in position control mode");
            _lbr.moveAsync(aSmartServoMotion);

            _theSmartServoRuntime = aSmartServoMotion.getRuntime();

            getLogger().info("Start SmartServo");
            // time measurement...
            StatisticTimer timing = new StatisticTimer();
            
            try
            {
                while (isLoop)
                {
                    // Timing - draw one step
                    OneTimeStep aStep = timing.newTimeStep();

                    _theSmartServoRuntime.updateWithRealtimeSystem();
                    
                     receiveNewPosition();
                    
                    // set target joint position
                    _theSmartServoRuntime.setDestination(_jointPosition);

                    aStep.end();

                }
            }
            catch (Exception e)
            {
                getLogger().info(e.getLocalizedMessage());
                e.printStackTrace();
            }
            
          
            ThreadUtil.milliSleep(1000);

            //Print statistics and parameters of the motion
            getLogger().info("Displaying final states after loop ");
            getLogger().info(getClass().getName() + _theSmartServoRuntime.toString());
            
            // Stop the motion
            getLogger().info("Stop the SmartServo motion");
            _theSmartServoRuntime.stopMotion();

            getLogger().info("Statistic Timing of Overall Loop " + timing);
            if (timing.getMeanTimeMillis() > 150)
            {
                getLogger().info("Statistic Timing is unexpected slow, you should try to optimize TCP/IP Transfer");
            }
        }
        
        

        
        /**
         * Initialize socket for UDP communication to an external client.
         * 
         * @return True, if socket opened successfully. False, otherwise.
         */
        public boolean initUDP()
        {
            try
            {
                _socket = new DatagramSocket(null);
                _socket.setReuseAddress(true);
                _socket.bind(new InetSocketAddress(PORT));
                _socket.setSoTimeout(SOCKET_TIMEOUT);
            }
            catch (Exception e)
            {
                getLogger().error("Problems initializing UDP socket: ", e);
                return false;
            }

            return true;
        }

        
        public void receiveNewPosition() throws IOException
        {
            byte[] receivedData = new byte[UDP_PACKET_SIZE];
            DatagramPacket receivedPacket = new DatagramPacket(receivedData, receivedData.length);

            try
            {
                _socket.receive(receivedPacket);
      
                // parse received DatagramPacket
                String s = new String(receivedPacket.getData(), 0, receivedPacket.getLength());
                if(!s.equals("StopServo"))
                {
                    String[] sList = s.split(",");
                    for(int i = 0; i < 7; i++)
                        _jointPosition.set(i, Double.valueOf(sList[i]));
                }
                else
                    isLoop = false;
                    
            }
            catch (SocketTimeoutException e)
            {
            }
        }
        
        
        /**
         * Call this if the instance of this class is not needed anymore.
         */
        public void dispose()
        {
            if (_socket != null)
            {
                _socket.close();
            }
        }
        
         
        /**
         * Main routine, which starts the application.
         * 
         * @param args
         *            arguments
         */
        public static void main(String[] args)
        {
            SmartServoJointTest app = new SmartServoJointTest();
            app.runApplication();
        }
}

　　在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可以控制机械臂沿直线随动：