MoveJustOnceLimbToPoseBody.groovy

MoveJustOnceLimbToPoseBody.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 poseAbsolute(def newAbsolutePose, def base, def tipList,DHParameterKinematics leg){
	try{

		def imuCenter = base.getIMUFromCentroid() 
		def newPoseTransformedToIMUCenter = newAbsolutePose.times(imuCenter.inverse())
		def newPoseAdjustedBacktoRobotCenterFrame = imuCenter.times(newPoseTransformedToIMUCenter)
		// Perform a pose opperation
		base.setGlobalToFiducialTransform(newPoseAdjustedBacktoRobotCenterFrame)
		
		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)
	}
}
void pose(def newAbsolutePose, def base, def tipList){
	DHParameterKinematics limb = base.getLegs().get(0)
	
	double inc = 0.1
	for(double i=inc;i<1;i+=inc){
		poseAbsolute( newAbsolutePose.scale(i),  base,  tipList,limb)
		ThreadUtil.wait(40)
	}
	for(double i=1;i>0;i-=inc){
		poseAbsolute( newAbsolutePose.scale(i),  base,  tipList,limb)
		ThreadUtil.wait(40)
	}
}

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);