KUKA iiwa — SmartServo随动
Posted 21207-ihome
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了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 }
在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模式开始仿真,实际的机械臂会接收虚拟机械臂发送的关节位置进行随动
以上是关于KUKA iiwa — SmartServo随动的主要内容,如果未能解决你的问题,请参考以下文章