public RobotTwoWheelState GetCurrentState(RobotTwoWheelCommand currentCommand) { RobotWheelModel rightW = new RobotWheelModel(rWheelPos, rWheelVel, rWheelAccel); RobotWheelModel leftW = new RobotWheelModel(lWheelPos, lWheelVel, lWheelAccel); RobotTwoWheelState state = new RobotTwoWheelState(curPose, currentCommand, rightW, leftW); return new RobotTwoWheelState(state, currentCommand); }
public RobotTwoWheelState(RobotTwoWheelState toCopy, RobotTwoWheelCommand command) { //deep copy the class this.pose = new RobotPose(toCopy.pose); this.rightWheel = new RobotWheelModel(toCopy.rightWheel); this.leftWheel = new RobotWheelModel(toCopy.leftWheel); this.command = command; }
public MonteCarloFollower(double lookAhead, double maxVelocity, double maxTurn) { DISTANCE_WEIGHT = 25; VELOCITY_WEIGHT = 1; TURN_WEIGHT = 1; this.lookAhead = lookAhead; this.maxVelocity = maxVelocity; this.maxTurn = maxTurn; state = new RobotTwoWheelState(new RobotPose(), new RobotTwoWheelCommand(0, 0), new RobotWheelModel(0, 0, 0), new RobotWheelModel(0, 0, 0)); }
public static RobotTwoWheelState Simulate(RobotTwoWheelCommand cmd, RobotTwoWheelState inititalState, double timestep) { //copy the initial state first! RobotTwoWheelState final = new RobotTwoWheelState(inititalState,cmd); double prevIntegratedLeft = inititalState.leftWheel.pos; double prevIntegratedRight = inititalState.rightWheel.pos; if (cmd.velocity > 2) cmd.velocity = 2; else if (cmd.velocity < -2) cmd.velocity = -2; if (cmd.turn > 360) cmd.turn = 360; else if (cmd.turn < -360) cmd.turn = -360; cmd.turn = cmd.turn * 0.5; double cmdLeftWheelVel = cmd.velocity - 0.5 * trackWidth * (cmd.turn * Math.PI / 180); double cmdRightWheelVel = cmd.velocity + 0.5 * trackWidth * (cmd.turn * Math.PI / 180); final.leftWheel.ApplyCommand(cmdLeftWheelVel, timestep); final.rightWheel.ApplyCommand(cmdRightWheelVel, timestep); //now figure out the updated pose given the new state of our robot.. double diffL = final.leftWheel.pos - prevIntegratedLeft; double diffR = final.rightWheel.pos - prevIntegratedRight; prevIntegratedLeft = final.leftWheel.pos; prevIntegratedRight = final.rightWheel.pos; //from lavalle's book ; http://planning.cs.uiuc.edu/node659.html //calculate the derivatives in measurements we have double xdot = ((diffL + diffR) / 2.0) * Math.Cos(inititalState.Pose.yaw); double ydot = ((diffL + diffR) / 2.0) * Math.Sin(inititalState.Pose.yaw); double headingDot = (diffR - diffL) / trackWidth; //update the state vector final.Pose.x = final.Pose.x + xdot; final.Pose.y = final.Pose.y + ydot; final.Pose.yaw = final.Pose.yaw + headingDot; while (final.Pose.yaw > Math.PI) final.Pose.yaw = final.Pose.yaw - 2 * Math.PI; while (final.Pose.yaw < -Math.PI) final.Pose.yaw = final.Pose.yaw + 2 * Math.PI; final.Pose.timestamp = final.Pose.timestamp + timestep; return final; }
public RRTNode(RobotTwoWheelState state) { this.state = state; this.parent = this; }
public RRTNode(RobotTwoWheelState state, RRTNode parent) { this.state = state; this.parent = parent; }
public static RobotTwoWheelState Simulate(RobotTwoWheelCommand cmd, RobotTwoWheelState inititalState, double timestep) { //copy the initial state first! RobotTwoWheelState final = new RobotTwoWheelState(inititalState, cmd); double prevIntegratedLeft = inititalState.leftWheel.pos; double prevIntegratedRight = inititalState.rightWheel.pos; if (cmd.velocity > 2) { cmd.velocity = 2; } else if (cmd.velocity < -2) { cmd.velocity = -2; } if (cmd.turn > 360) { cmd.turn = 360; } else if (cmd.turn < -360) { cmd.turn = -360; } cmd.turn = cmd.turn * 0.5; double cmdLeftWheelVel = cmd.velocity - 0.5 * trackWidth * (cmd.turn * Math.PI / 180); double cmdRightWheelVel = cmd.velocity + 0.5 * trackWidth * (cmd.turn * Math.PI / 180); final.leftWheel.ApplyCommand(cmdLeftWheelVel, timestep); final.rightWheel.ApplyCommand(cmdRightWheelVel, timestep); //now figure out the updated pose given the new state of our robot.. double diffL = final.leftWheel.pos - prevIntegratedLeft; double diffR = final.rightWheel.pos - prevIntegratedRight; prevIntegratedLeft = final.leftWheel.pos; prevIntegratedRight = final.rightWheel.pos; //from lavalle's book ; http://planning.cs.uiuc.edu/node659.html //calculate the derivatives in measurements we have double xdot = ((diffL + diffR) / 2.0) * Math.Cos(inititalState.Pose.yaw); double ydot = ((diffL + diffR) / 2.0) * Math.Sin(inititalState.Pose.yaw); double headingDot = (diffR - diffL) / trackWidth; //update the state vector final.Pose.x = final.Pose.x + xdot; final.Pose.y = final.Pose.y + ydot; final.Pose.yaw = final.Pose.yaw + headingDot; while (final.Pose.yaw > Math.PI) { final.Pose.yaw = final.Pose.yaw - 2 * Math.PI; } while (final.Pose.yaw < -Math.PI) { final.Pose.yaw = final.Pose.yaw + 2 * Math.PI; } final.Pose.timestamp = final.Pose.timestamp + timestep; return(final); }