moveDh.groovy

import com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics;
import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.util.ThreadUtil;

import com.neuronrobotics.bowlerstudio.creature.CreatureLab;
import org.apache.commons.io.IOUtils;

//Create the kinematics model from the xml file describing the D-H compliant parameters. 
String xmlContent = ScriptingEngine.codeFromGistID("2b0cff20ccee085c9c36","TrobotLinks.xml")[0];

MobileBase base=null;
if(DeviceManager.getSpecificDevice(MobileBase.class, "TrobotArmGroup")==null){
	BowlerStudio.speak("I did not fine a device called TrobotArmGroup. Connecting TrobotArmGroup.");
	base = new MobileBase(IOUtils.toInputStream(xmlContent, "UTF-8"));
	DeviceManager.addConnection(base,"TrobotArmGroup");
}else{
	base = (MobileBase)DeviceManager.getSpecificDevice(MobileBase.class, "TrobotArmGroup");
}

DHParameterKinematics DHArm = base.getAppendages().get(0); 

double [] values = DHArm.getCurrentJointSpaceVector();
for(int i=0;i<values.length;i++){
	values[i]=0;
}
DHArm.setDesiredJointSpaceVector(values, 2);
ThreadUtil.wait(3000);
TransformNR start = DHArm.getCurrentTaskSpaceTransform();
double x= start.getX();
double y=start.getY();
double z=start.getZ();
for(int i=-1;i< 1;i++){
	for(int j=-1;j<1;j++){
		TransformNR tr = new TransformNR(	
			x+(10*i)+(10*j),
 			y+(10*i)+(10*j),
			z,
			new RotationNR());
		println(tr)
		DHArm.setDesiredTaskSpaceTransform(tr, 2);
		ThreadUtil.wait(3000);
	}
}
for(int i=-1;i< 1;i++){
	TransformNR tr = new TransformNR(	
		x,
 		y,
		z+(50*i),
		new RotationNR());
	println(tr)
	DHArm.setDesiredTaskSpaceTransform(tr, 2);
	ThreadUtil.wait(4000);
	
}
DHArm.setDesiredTaskSpaceTransform(start, 2);