Apply transformations to the base to move all of the limbs
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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){ | |
| 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) | |
| } | |
| } | |
| void pose(def newAbsolutePose, def base, def tipList){ | |
| double inc = 0.1 | |
| for(double i=inc;i<1;i+=inc){ | |
| poseAbsolute( newAbsolutePose.scale(i), base, tipList) | |
| ThreadUtil.wait(40) | |
| } | |
| for(double i=1;i>0;i-=inc){ | |
| poseAbsolute( newAbsolutePose.scale(i), base, tipList) | |
| 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); |