poseBaseSimpleGist.groovy

poseBaseSimpleGist.groovy

def base;
//Check if the device already exists in the device Manager
if(args==null){
	base=DeviceManager.getSpecificDevice( "MediumKat",{ScriptingEngine.gitScriptRun(	"https://github.com/OperationSmallKat/SmallKat_V2.git", "loadRobot.groovy", [ "https://github.com/OperationSmallKat/greycat.git", "MediumKat.xml","GameController_22"] )})
}else
	base=args.get(0)

HashMap<DHParameterKinematics,TransformNR > getTipLocations(def base){
	def legs = base.getLegs() 
	def tipList = new HashMap<DHParameterKinematics,TransformNR >()
	for(def leg:legs){
		// Read the location of the foot before moving the body
		def home =leg.getCurrentTaskSpaceTransform()
		tipList.put(leg,home)
	}
	return tipList
}


void pose(def newAbsolutePose, def base, def tipList){
	def legs = base.getLegs() 
	try{
		// Perform a pose opperation
		base.setGlobalToFiducialTransform(newAbsolutePose)
		for(def leg:legs){
			def pose =tipList.get(leg)
			if(leg.checkTaskSpaceTransform(pose))// move the leg only is the pose of hte limb is possible
				leg.setDesiredTaskSpaceTransform(pose, 0);// set leg to the location of where the foot was
			else{
				println "ERROR leg "+leg.getScriptingName()+" can not go to pose "+pose
				BowlerStudio.printStackTrace(new RuntimeException())
			}
		}
	}catch (Throwable t){
		BowlerStudio.printStackTrace(t)
	}
	Thread.sleep(1000)
}

def startPose = base.getFiducialToGlobalTransform()

def tips = getTipLocations( base)

pose(new TransformNR(),base,tips)

pose(new TransformNR(0,0,-10,new RotationNR(5,0,0)),base,tips)

pose(new TransformNR(0,0,-10,new RotationNR(-5,0,0)),base,tips)

pose(new TransformNR(0,0,-10,new RotationNR(0,5,0)),base,tips)

pose(new TransformNR(0,0,-10,new RotationNR(0,-5,0)),base,tips)

pose(new TransformNR(0,0,-10,new RotationNR(0,0,0)),base,tips)

base.DriveArc(new TransformNR(), 0.1);