printDHTable.groovy

.gitignore

/.classpath
/.project
**.class

printDHTable.groovy

import com.neuronrobotics.bowlerstudio.creature.MobileBaseLoader
import com.neuronrobotics.bowlerstudio.scripting.ScriptingEngine
import com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics
import com.neuronrobotics.sdk.addons.kinematics.MobileBase
import com.neuronrobotics.sdk.common.DeviceManager

//Your code here

MobileBase base = (MobileBase)DeviceManager.getSpecificDevice( "HephaestusArmV2",{
			//If the device does not exist, prompt for the connection
			
			MobileBase m = MobileBaseLoader.fromGit(
				"https://github.com/Hephaestus-Arm/HephaestusArm2.git",
				"hephaestus.xml"
				)
			if(m==null)
				throw new RuntimeException("Arm failed to assemble itself")
			println "Connecting new device robot arm "+m
			return m
		})

for(DHParameterKinematics limb:base.getAllDHChains()) {
	println limb.getScriptingName()
	println "Home pose "+limb.calcHome();
	println "Root of the limb"+limb.getRobotToFiducialTransform()
	for(int i=0;i<limb.getNumberOfLinks();i++) {
		println "Link "+i+" A="+limb.getDH_R(i)+"mm D="+limb.getDH_D(i)+"mm Alpha="+Math.toDegrees(limb.getDH_Alpha(i))+"' Theta="+Math.toDegrees(limb.getDH_Theta(i))+"'"
	}
}