poseAroundAPoint.groovy

.gitignore

/.project
/.classpath
/.cproject
/cache/
/*.class

poseAroundAPoint.groovy

import com.neuronrobotics.bowlerstudio.BowlerStudio
import com.neuronrobotics.bowlerstudio.scripting.ScriptingEngine
import com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics
import com.neuronrobotics.sdk.addons.kinematics.MobileBase
import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR
import com.neuronrobotics.sdk.common.DeviceManager

MobileBase 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(MobileBase base){
	def legs = base.getLegs() 
	def tipList = new HashMap<DHParameterKinematics,TransformNR >()
	for(DHParameterKinematics 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, MobileBase base, def tipList){
	def legs = base.getLegs() 
	try{

		def imuCenter = base.getIMUFromCentroid() 
		def newPoseTransformedToIMUCenter = newAbsolutePose.times(imuCenter.inverse())
		def newPoseAdjustedBacktoRobotCenterFrame = imuCenter.times(newPoseTransformedToIMUCenter)
		// Perform a pose opperation
		base.setGlobalToFiducialTransform(newPoseAdjustedBacktoRobotCenterFrame)
		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);