資源簡介
在機器人軟件平臺上建立一個包含若干個靜止障礙物和運動障礙物的仿真環境，設定機器人的起始點和終點后，機器人能夠規劃出一條從起始點到目標點的安全路徑。查閱相關路徑規劃算法，實現一種以上算法并相互比較。要求給出源代碼、試驗結果并且進行演示。
代碼片段和文件信息
package?myrobot;
?
import?javax.vecmath.Point3d;?
import?javax.vecmath.Vector2d;?
import?javax.vecmath.Vector3d;?
import?javax.vecmath.Vector3f;?
?
?
import?simbad.sim.*;?
import?simbad.demo.*;?
import?simbad.gui.Simbad;?
??
public?class?MyRobot?extends?Demo?
{?
//?全局目標坐標?
Vector2d?goal?=?new?Vector2d（88?）;?
Vector3d?goal3d?=?new?Vector3d（8?08）;?
?
private?static?final?double?repelConstant?=?100.0;//?斥力系數?
private?static?final?double?attractConstant?=?30.0;//?引力系數?
private?boolean?debug?=?false;//?調試標記?
?
public?class?Robot?extends?Agent?
{?
?
RangeSensorBelt?sonarsbumpers;?
LampActuator?lamp;?
?
private?Vector3d?origin?=?null;?
?
public?void?initBehavior（）?
{??
}?
?
public?Robot（Vector3d?position?String?name）?
{?
super（position?name）;?
?
bumpers?=?RobotFactory.addBumperBeltSensor（this）; ?
sonars?=?RobotFactory.addSonarBeltSensor（this）;?
lamp?=?RobotFactory.addLamp（this）;?
origin?=?position;//?起點位置?
?
}?
?
public?Vector3d?getVelocity（）?
{?
return?this.linearVelocity;?//線速度
}?
?
private?int?getQuadrant（Vector2d?vector）?//計算向量的象限
{?
double?x?=?vector.x;?
double?y?=?vector.y;?
if?（x?>?0?&&?y?>?0）//?第一象限?
{?
return?1;?
}?else?if?（x??0）//?第二象限?
{?
return?2;?
}?else?if?（x?<0?&&?y? {?
return?3;?
}?else?if?（x?>?0?&&?y?<0）//?第四象限?
{?
return?4;?
}?else?if?（x?>?0?&&?y?==?0）//?x正半軸?
{?
return?-1;?
}?else?if?（x?==?0?&&?y?>?0）//?y正半軸?
{?
return?-2;?
}?else?if?（x?<0?&&?y?==?0）//?x負半軸?
{?
return?-3;?
}?else?if?（x?==?0?&&?y?<0）//?y負半軸?
{?
return?-4;?
}?else?
{?
return?0;?
}?
}?
?
private?double?getAngle（Vector2d?v1?Vector2d?v2）?//計算兩個向量之間的弧度角
{?
?
double?k?=?v1.y?/?v1.x;?
double?y?=?k?*?v2.x;?
switch?（getQuadrant（v1））?
{?
case?1:?
case?4:?
case?-1:?
if?（v2.y?>?y）?
{?
return?v1.angle（v2）;?//兩個向量之間的夾角弧度
}?else?if?（v2.y? {?
return?2?*?Math.PI?-?v1.angle（v2）;?
}?else?
{?
if?（v1.x?*?v2.x? {?
return?Math.PI;
}?else?
????????????????????????????????????????{
if?（debug）?
System.out.println（“NO“）;?
return?0;?
}?
}?
case?2:?
case?3:?
case?-3:?
if?（v2.y?>?y）?
{?
return?2?*?Math.PI?-?v1.angle（v2）;?
}?else?if?（v2.y? {?
return?v1.angle（v2）;?
}?else?
{?
if?（v1.x?*?v2.x? {?
return?Math.PI;?
}?else?
????????????????????????????????????????{?
if?（debug）?
System.out.println（“here“）;?
return?0;?
}?
}?
case?-2:?
int?i?=?getQuadrant（v2）;?
if?（i?==?-4）?
{?
return?Math.PI;?
}?else?if?（i?==?-2?||?i?==?-1?||?i?==?1?||?i?==?4）?
{?
return?2?*?Math.PI?-?v1.angle（v2）;?
}?else?
{?
return?v1.angle（v2）;?
?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2013-01-07?18:30??MyRobot\
?????文件????????3716??2013-01-07?18:27??MyRobot\build.xm
?????文件??????????85??2013-01-07?18:27??MyRobot\manifest.mf
?????目錄???????????0??2013-01-07?18:27??MyRobot\nbproject\
?????文件???????78029??2013-01-07?18:27??MyRobot\nbproject\build-impl.xm
?????文件?????????475??2013-01-07?18:27??MyRobot\nbproject\genfiles.properties
?????文件????????2332??2013-01-07?18:27??MyRobot\nbproject\project.properties
?????文件?????????515??2013-01-07?18:27??MyRobot\nbproject\project.xm
?????目錄???????????0??2013-01-07?18:28??MyRobot\src\
?????目錄???????????0??2013-01-07?18:27??MyRobot\src\myrobot\
?????文件???????11506??2013-01-07?19:09??MyRobot\src\myrobot\MyRobot.java
?????目錄???????????0??2013-01-07?18:28??MyRobot\src\simbad\
?????目錄???????????0??2013-01-07?18:28??MyRobot\src\simbad\demo\
?????文件????????4138??2013-01-07?18:28??MyRobot\src\simbad\demo\AvoidersDemo.java
?????文件????????3106??2013-01-07?18:28??MyRobot\src\simbad\demo\ba
?????文件????????4078??2013-01-07?18:28??MyRobot\src\simbad\demo\bl
?????文件????????4149??2013-01-07?18:28??MyRobot\src\simbad\demo\BumpersDemo.java
?????文件????????1190??2013-01-07?18:28??MyRobot\src\simbad\demo\Demo.java
?????文件????????2706??2013-01-07?18:28??MyRobot\src\simbad\demo\DemoManager.java
?????文件????????3440??2013-01-07?18:28??MyRobot\src\simbad\demo\DifferentialKinematicDemo.java
?????文件????????4506??2013-01-07?18:28??MyRobot\src\simbad\demo\ImagerDemo.java
?????文件????????2103??2013-01-07?18:28??MyRobot\src\simbad\demo\KheperaDemo.java
?????文件????????3509??2013-01-07?18:28??MyRobot\src\simbad\demo\LightSearchDemo.java
?????文件????????2737??2013-01-07?18:28??MyRobot\src\simbad\demo\PickCherriesDemo.java
?????文件????????3272??2013-01-07?18:28??MyRobot\src\simbad\demo\PushBallsDemo.java
?????文件????????1895??2013-01-07?18:28??MyRobot\src\simbad\demo\SimplestDemo.java
?????文件????????4464??2013-01-07?18:28??MyRobot\src\simbad\demo\SingleAvoiderDemo.java
?????目錄???????????0??2013-01-07?18:28??MyRobot\src\simbad\gui\
?????文件????????2444??2013-01-07?18:28??MyRobot\src\simbad\gui\AgentFollower.java
?????文件????????5673??2013-01-07?18:28??MyRobot\src\simbad\gui\AgentInspector.java
?????文件????????3534??2013-01-07?18:28??MyRobot\src\simbad\gui\Console.java
............此處省略50個文件信息
評論
共有 條評論
川公網安備 51152502000135號