plot demo for version 2
mcarfagno
parent
0f572d0caa
commit
19532431dc
|
|
@ -0,0 +1,104 @@
|
||||||
|
import numpy as np
|
||||||
|
np.seterr(divide='ignore', invalid='ignore')
|
||||||
|
|
||||||
|
from scipy.integrate import odeint
|
||||||
|
from scipy.interpolate import interp1d
|
||||||
|
import cvxpy as cp
|
||||||
|
|
||||||
|
from utils import road_curve, f, df
|
||||||
|
|
||||||
|
from mpc_config import Params
|
||||||
|
P=Params()
|
||||||
|
|
||||||
|
def get_linear_model(x_bar,u_bar):
|
||||||
|
"""
|
||||||
|
"""
|
||||||
|
|
||||||
|
x = x_bar[0]
|
||||||
|
y = x_bar[1]
|
||||||
|
theta = x_bar[2]
|
||||||
|
|
||||||
|
v = u_bar[0]
|
||||||
|
w = u_bar[1]
|
||||||
|
|
||||||
|
A = np.zeros((P.N,P.N))
|
||||||
|
A[0,2]=-v*np.sin(theta)
|
||||||
|
A[1,2]=v*np.cos(theta)
|
||||||
|
A_lin=np.eye(P.N)+P.dt*A
|
||||||
|
|
||||||
|
B = np.zeros((P.N,P.M))
|
||||||
|
B[0,0]=np.cos(theta)
|
||||||
|
B[1,0]=np.sin(theta)
|
||||||
|
B[2,1]=1
|
||||||
|
B_lin=P.dt*B
|
||||||
|
|
||||||
|
f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w]).reshape(P.N,1)
|
||||||
|
C_lin = P.dt*(f_xu - np.dot(A,x_bar.reshape(P.N,1)) - np.dot(B,u_bar.reshape(P.M,1)))
|
||||||
|
|
||||||
|
return A_lin,B_lin,C_lin
|
||||||
|
|
||||||
|
|
||||||
|
def optimize(state,u_bar,track):
|
||||||
|
'''
|
||||||
|
:param state:
|
||||||
|
:param u_bar:
|
||||||
|
:param track:
|
||||||
|
:returns:
|
||||||
|
'''
|
||||||
|
|
||||||
|
MAX_SPEED = 1.25
|
||||||
|
MIN_SPEED = 0.75
|
||||||
|
MAX_STEER_SPEED = 1.57/2
|
||||||
|
|
||||||
|
# compute polynomial coefficients of the track
|
||||||
|
K=road_curve(state,track)
|
||||||
|
|
||||||
|
# dynamics starting state w.r.t vehicle frame
|
||||||
|
x_bar=np.zeros((P.N,P.T+1))
|
||||||
|
|
||||||
|
#prediction for linearization of costrains
|
||||||
|
for t in range (1,P.T+1):
|
||||||
|
xt=x_bar[:,t-1].reshape(P.N,1)
|
||||||
|
ut=u_bar[:,t-1].reshape(P.M,1)
|
||||||
|
A,B,C=get_linear_model(xt,ut)
|
||||||
|
xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)
|
||||||
|
x_bar[:,t]= xt_plus_one
|
||||||
|
|
||||||
|
#CVXPY Linear MPC problem statement
|
||||||
|
cost = 0
|
||||||
|
constr = []
|
||||||
|
x = cp.Variable((P.N, P.T+1))
|
||||||
|
u = cp.Variable((P.M, P.T))
|
||||||
|
|
||||||
|
for t in range(P.T):
|
||||||
|
|
||||||
|
#cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi
|
||||||
|
cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi
|
||||||
|
cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte
|
||||||
|
|
||||||
|
# Actuation rate of change
|
||||||
|
if t < (P.T - 1):
|
||||||
|
cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(P.M))
|
||||||
|
|
||||||
|
# Actuation effort
|
||||||
|
cost += cp.quad_form( u[:, t],1*np.eye(P.M))
|
||||||
|
|
||||||
|
# Kinrmatics Constrains (Linearized model)
|
||||||
|
A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])
|
||||||
|
constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]
|
||||||
|
|
||||||
|
# sums problem objectives and concatenates constraints.
|
||||||
|
constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition
|
||||||
|
constr += [u[0, :] <= MAX_SPEED]
|
||||||
|
constr += [u[0, :] >= MIN_SPEED]
|
||||||
|
constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]
|
||||||
|
|
||||||
|
# Solve
|
||||||
|
prob = cp.Problem(cp.Minimize(cost), constr)
|
||||||
|
solution = prob.solve(solver=cp.OSQP, verbose=False)
|
||||||
|
|
||||||
|
#retrieved optimized U and assign to u_bar to linearize in next step
|
||||||
|
u_bar=np.vstack((np.array(u.value[0, :]).flatten(),
|
||||||
|
(np.array(u.value[1, :]).flatten())))
|
||||||
|
|
||||||
|
return u_bar
|
||||||
Binary file not shown.
|
After Width: | Height: | Size: 6.2 KiB |
|
|
@ -0,0 +1,15 @@
|
||||||
|
newmtl Material
|
||||||
|
Ns 10.0000
|
||||||
|
Ni 1.5000
|
||||||
|
d 1.0000
|
||||||
|
Tr 0.0000
|
||||||
|
Tf 1.0000 1.0000 1.0000
|
||||||
|
illum 2
|
||||||
|
Ka 0.0000 0.0000 0.0000
|
||||||
|
Kd 0.5880 0.5880 0.5880
|
||||||
|
Ks 0.0000 0.0000 0.0000
|
||||||
|
Ke 0.0000 0.0000 0.0000
|
||||||
|
map_Ka cube.tga
|
||||||
|
map_Kd checker_blue.png
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,18 @@
|
||||||
|
# Blender v2.66 (sub 1) OBJ File: ''
|
||||||
|
# www.blender.org
|
||||||
|
mtllib plane.mtl
|
||||||
|
o Plane
|
||||||
|
v 15.000000 -15.000000 0.000000
|
||||||
|
v 15.000000 15.000000 0.000000
|
||||||
|
v -15.000000 15.000000 0.000000
|
||||||
|
v -15.000000 -15.000000 0.000000
|
||||||
|
|
||||||
|
vt 15.000000 0.000000
|
||||||
|
vt 15.000000 15.000000
|
||||||
|
vt 0.000000 15.000000
|
||||||
|
vt 0.000000 0.000000
|
||||||
|
|
||||||
|
usemtl Material
|
||||||
|
s off
|
||||||
|
f 1/1 2/2 3/3
|
||||||
|
f 1/1 3/3 4/4
|
||||||
|
|
@ -0,0 +1,29 @@
|
||||||
|
<?xml version="0.0" ?>
|
||||||
|
<robot name="plane">
|
||||||
|
<link name="planeLink">
|
||||||
|
<contact>
|
||||||
|
<lateral_friction value="1"/>
|
||||||
|
</contact>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<mass value=".0"/>
|
||||||
|
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="plane.obj" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<plane normal="0 0 1"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
</robot>
|
||||||
|
|
||||||
|
|
@ -0,0 +1,827 @@
|
||||||
|
<?xml version="1.0" ?>
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- | This document was autogenerated by xacro from robots/kobuki_hexagons_kinect.urdf.xacro | -->
|
||||||
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!--
|
||||||
|
- Base : kobuki
|
||||||
|
- Stacks : hexagons
|
||||||
|
- 3d Sensor : kinect
|
||||||
|
-->
|
||||||
|
<robot name="turtlebot" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<link name="base_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<!-- new mesh -->
|
||||||
|
<mesh filename="turtlebot/kobuki_description/meshes/main_body.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 0 0" xyz="0.001 0 0.05199"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.10938" radius="0.176"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 0 0" xyz="0.0 0 0.05949"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="2.4"/>
|
||||||
|
<!-- 2.4/2.6 kg for small/big battery pack -->
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="wheel_left_joint" type="continuous">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="wheel_left_link"/>
|
||||||
|
<origin rpy="-1.57079632679 0 0" xyz="0.00 0.115 0.0250"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
</joint>
|
||||||
|
<link name="wheel_left_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/kobuki_description/meshes/wheel.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0206" radius="0.0352"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.1"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
|
||||||
|
</inertial>
|
||||||
|
<contact>
|
||||||
|
<lateral_friction value="1.0"/>
|
||||||
|
<rolling_friction value="0.0"/>
|
||||||
|
<stiffness value="30000"/>
|
||||||
|
<damping value="1000"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint name="wheel_right_joint" type="continuous">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="wheel_right_link"/>
|
||||||
|
<origin rpy="-1.57079632679 0 0" xyz="0.00 -0.115 0.0250"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
</joint>
|
||||||
|
<link name="wheel_right_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/kobuki_description/meshes/wheel.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0206" radius="0.0350"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.1"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
|
||||||
|
</inertial>
|
||||||
|
<contact>
|
||||||
|
<lateral_friction value="1.0"/>
|
||||||
|
<rolling_friction value="0.0"/>
|
||||||
|
<stiffness value="30000"/>
|
||||||
|
<damping value="10000"/>
|
||||||
|
</contact>
|
||||||
|
|
||||||
|
</link>
|
||||||
|
<joint name="caster_front_joint" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="caster_front_link"/>
|
||||||
|
<origin rpy="-1.57079632679 0 0" xyz="0.115 0.0 0.007"/>
|
||||||
|
</joint>
|
||||||
|
<link name="caster_front_link">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0176" radius="0.017"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.01"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
|
||||||
|
</inertial>
|
||||||
|
<contact>
|
||||||
|
<lateral_friction value="0.0"/>
|
||||||
|
<rolling_friction value="0.0"/>
|
||||||
|
<stiffness value="30000"/>
|
||||||
|
<damping value="10000"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint name="caster_back_joint" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="caster_back_link"/>
|
||||||
|
<origin rpy="-1.57079632679 0 0" xyz="-0.135 0.0 0.009"/>
|
||||||
|
</joint>
|
||||||
|
<link name="caster_back_link">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0176" radius="0.017"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.01"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
|
||||||
|
</inertial>
|
||||||
|
<contact>
|
||||||
|
<lateral_friction value="0.0"/>
|
||||||
|
<rolling_friction value="0.0"/>
|
||||||
|
<stiffness value="30000"/>
|
||||||
|
<damping value="1000"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint name="gyro_joint" type="fixed">
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0.056 0.062 0.0202"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="gyro_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="gyro_link">
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="cliff_sensor_left_joint" type="fixed">
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0.08734 0.13601 0.0214"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="cliff_sensor_left_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="cliff_sensor_left_link">
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.0001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="cliff_sensor_right_joint" type="fixed">
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0.085 -0.13601 0.0214"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="cliff_sensor_right_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="cliff_sensor_right_link">
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.0001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="cliff_sensor_front_joint" type="fixed">
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0.156 0.00 0.0214"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="cliff_sensor_front_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="cliff_sensor_front_link">
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.0001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<gazebo>
|
||||||
|
<controller:gazebo_ros_kobuki name="kobuki_controller" plugin="libgazebo_ros_kobuki.so">
|
||||||
|
<left_wheel_joint_name>wheel_left_joint</left_wheel_joint_name>
|
||||||
|
<right_wheel_joint_name>wheel_right_joint</right_wheel_joint_name>
|
||||||
|
<wheel_separation>.230</wheel_separation>
|
||||||
|
<wheel_diameter>0.070</wheel_diameter>
|
||||||
|
<torque>1.0</torque>
|
||||||
|
<velocity_command_timeout>0.6</velocity_command_timeout>
|
||||||
|
<cliff_sensor_left_name>cliff_sensor_left</cliff_sensor_left_name>
|
||||||
|
<cliff_sensor_front_name>cliff_sensor_front</cliff_sensor_front_name>
|
||||||
|
<cliff_sensor_right_name>cliff_sensor_right</cliff_sensor_right_name>
|
||||||
|
<cliff_detection_threshold>0.04</cliff_detection_threshold>
|
||||||
|
<bumper_name>bumpers</bumper_name>
|
||||||
|
<base_collision_model_link>base_link</base_collision_model_link>
|
||||||
|
</controller:gazebo_ros_kobuki>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="wheel_left_link">
|
||||||
|
<mu1 value="10"/>
|
||||||
|
<mu2 value="10"/>
|
||||||
|
<kp value="100000000.0"/>
|
||||||
|
<kd value="10000.0"/>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="wheel_right_link">
|
||||||
|
<mu1 value="10"/>
|
||||||
|
<mu2 value="10"/>
|
||||||
|
<kp value="100000000.0"/>
|
||||||
|
<kd value="10000.0"/>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="caster_front_link">
|
||||||
|
<mu1 value="0"/>
|
||||||
|
<mu2 value="0"/>
|
||||||
|
<kp value="100000000.0"/>
|
||||||
|
<kd value="10000.0"/>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="caster_back_link">
|
||||||
|
<mu1 value="0"/>
|
||||||
|
<mu2 value="0"/>
|
||||||
|
<kp value="100000000.0"/>
|
||||||
|
<kd value="10000.0"/>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="base_link">
|
||||||
|
<sensor:contact name="bumpers">
|
||||||
|
<geom>base_footprint_geom_base_link</geom>
|
||||||
|
<topic>bumpers</topic>
|
||||||
|
<alwaysOn>true</alwaysOn>
|
||||||
|
<updateRate>50</updateRate>
|
||||||
|
</sensor:contact>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="cliff_sensor_left_link">
|
||||||
|
<sensor:ray name="cliff_sensor_left">
|
||||||
|
<alwaysOn>true</alwaysOn>
|
||||||
|
<updateRate>50</updateRate>
|
||||||
|
<rayCount>50</rayCount>
|
||||||
|
<rangeCount>1</rangeCount>
|
||||||
|
<resRange>1.0</resRange>
|
||||||
|
<minAngle>-0.04361</minAngle>
|
||||||
|
<maxAngle>0.04361</maxAngle>
|
||||||
|
<minRange>0.01</minRange>
|
||||||
|
<maxRange>0.15</maxRange>
|
||||||
|
<displayRays>true</displayRays>
|
||||||
|
</sensor:ray>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="cliff_sensor_right_link">
|
||||||
|
<sensor:ray name="cliff_sensor_right">
|
||||||
|
<alwaysOn>true</alwaysOn>
|
||||||
|
<updateRate>50</updateRate>
|
||||||
|
<rayCount>50</rayCount>
|
||||||
|
<rangeCount>1</rangeCount>
|
||||||
|
<resRange>1.0</resRange>
|
||||||
|
<minAngle>-2.5</minAngle>
|
||||||
|
<maxAngle>2.5</maxAngle>
|
||||||
|
<minRange>0.01</minRange>
|
||||||
|
<maxRange>0.15</maxRange>
|
||||||
|
<displayRays>true</displayRays>
|
||||||
|
</sensor:ray>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="cliff_sensor_front_link">
|
||||||
|
<sensor:ray name="cliff_sensor_front">
|
||||||
|
<alwaysOn>true</alwaysOn>
|
||||||
|
<updateRate>50</updateRate>
|
||||||
|
<rayCount>50</rayCount>
|
||||||
|
<rangeCount>1</rangeCount>
|
||||||
|
<resRange>1.0</resRange>
|
||||||
|
<minAngle>-2.5</minAngle>
|
||||||
|
<maxAngle>2.5</maxAngle>
|
||||||
|
<minRange>0.01</minRange>
|
||||||
|
<maxRange>0.15</maxRange>
|
||||||
|
<displayRays>true</displayRays>
|
||||||
|
</sensor:ray>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo>
|
||||||
|
<controller:gazebo_ros_imu name="imu_controller" plugin="libgazebo_ros_imu.so">
|
||||||
|
<alwaysOn>true</alwaysOn>
|
||||||
|
<updateRate>50</updateRate>
|
||||||
|
<bodyName>gyro_link</bodyName>
|
||||||
|
<topicName>/mobile_base/sensors/imu_data</topicName>
|
||||||
|
<gaussianNoise>2.89e-06</gaussianNoise>
|
||||||
|
<xyzOffsets>0 0 0</xyzOffsets>
|
||||||
|
<rpyOffsets>0 0 0</rpyOffsets>
|
||||||
|
<interface:position name="gyro_link"/>
|
||||||
|
</controller:gazebo_ros_imu>
|
||||||
|
</gazebo>
|
||||||
|
<joint name="pole_bottom_0_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0.12 0.082 0.1028"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_bottom_0_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_bottom_0_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0492" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_bottom_1_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0.055 0.12 0.1028"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_bottom_1_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_bottom_1_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0492" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_bottom_2_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.055 0.12 0.1028"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_bottom_2_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_bottom_2_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0492" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_bottom_3_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0.12 -0.082 0.1028"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_bottom_3_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_bottom_3_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0492" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_bottom_4_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0.055 -0.12 0.1028"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_bottom_4_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_bottom_4_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0492" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_bottom_5_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.055 -0.12 0.1028"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_bottom_5_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_bottom_5_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0492" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="plate_bottom_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0.02364 0.0 0.1306"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="plate_bottom_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="plate_bottom_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_bottom.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.26727 0.340 0.006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.01"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_middle_0_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0.0381 0.1505 0.164"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_middle_0_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_middle_0_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_middle.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0608" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_middle_1_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0.0381 -0.1505 0.164"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_middle_1_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_middle_1_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_middle.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0608" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_middle_2_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.0381 0.1505 0.164"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_middle_2_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_middle_2_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_middle.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0608" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_middle_3_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.0381 -0.1505 0.164"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_middle_3_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_middle_3_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_middle.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0608" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="plate_middle_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.01364 0.0 0.1874"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="plate_middle_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="plate_middle_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_middle.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.28727 0.340 0.006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.01"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_top_0_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0.0381 0.1505 0.292"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_top_0_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_top_0_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz=" 0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_top.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.2032" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_top_1_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0.0381 -0.1505 0.292"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_top_1_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_top_1_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz=" 0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_top.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.2032" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_top_2_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.0381 0.1505 0.292"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_top_2_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_top_2_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz=" 0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_top.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.2032" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_top_3_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.0381 -0.1505 0.292"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_top_3_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_top_3_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz=" 0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_top.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.2032" radius=".006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_kinect_0_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.1024 0.098 0.2372"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_kinect_0_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_kinect_0_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<!-- <origin xyz="0 0 0" rpy="${-M_PI/2} ${M_PI} 0"/> -->
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_kinect.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0936" radius="0.006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pole_kinect_1_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.1024 -0.098 0.2372"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="pole_kinect_1_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pole_kinect_1_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<!-- <origin xyz="0 0 0" rpy="${-M_PI/2} ${M_PI} 0"/> -->
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_kinect.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0936" radius="0.006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="plate_top_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.01364 0.0 0.3966"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="plate_top_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="plate_top_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_top.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.28727 0.340 0.006"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.01"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="camera_rgb_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.087 -0.0125 0.287"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="camera_rgb_frame"/>
|
||||||
|
</joint>
|
||||||
|
<link name="camera_rgb_frame">
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="camera_rgb_optical_joint" type="fixed">
|
||||||
|
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
|
||||||
|
<parent link="camera_rgb_frame"/>
|
||||||
|
<child link="camera_rgb_optical_frame"/>
|
||||||
|
</joint>
|
||||||
|
<link name="camera_rgb_optical_frame">
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="camera_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.031 0.0125 -0.016"/>
|
||||||
|
<parent link="camera_rgb_frame"/>
|
||||||
|
<child link="eyes"/>
|
||||||
|
</joint>
|
||||||
|
<link name="eyes">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="turtlebot/turtlebot_description/meshes/sensors/kinect.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.0315 0.0 -0.017"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.07271 0.27794 0.073"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="camera_depth_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.025 0"/>
|
||||||
|
<parent link="camera_rgb_frame"/>
|
||||||
|
<child link="camera_depth_frame"/>
|
||||||
|
</joint>
|
||||||
|
<link name="camera_depth_frame">
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="camera_depth_optical_joint" type="fixed">
|
||||||
|
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
|
||||||
|
<parent link="camera_depth_frame"/>
|
||||||
|
<child link="camera_depth_optical_frame"/>
|
||||||
|
</joint>
|
||||||
|
<link name="camera_depth_optical_frame">
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
</robot>
|
||||||
Binary file not shown.
|
After Width: | Height: | Size: 559 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 92 KiB |
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 14 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 44 KiB |
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 82 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 69 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 178 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 84 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 132 KiB |
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
|
|
@ -0,0 +1,6 @@
|
||||||
|
class Params():
|
||||||
|
def __init__(self):
|
||||||
|
self.N = 3 #number of state variables
|
||||||
|
self.M = 2 #number of control variables
|
||||||
|
self.T = 20 #Prediction Horizon
|
||||||
|
self.dt = 0.25 #discretization step
|
||||||
|
|
@ -0,0 +1,207 @@
|
||||||
|
#! /usr/bin/env python
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
from matplotlib import animation
|
||||||
|
|
||||||
|
from utils import compute_path_from_wp
|
||||||
|
from cvxpy_mpc import optimize
|
||||||
|
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
|
||||||
|
# Robot Starting position
|
||||||
|
SIM_START_X=0
|
||||||
|
SIM_START_Y=0.5
|
||||||
|
SIM_START_H=0
|
||||||
|
|
||||||
|
from mpc_config import Params
|
||||||
|
P=Params()
|
||||||
|
|
||||||
|
# Classes
|
||||||
|
class MPC():
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
|
||||||
|
# State for the robot mathematical model [x,y,heading]
|
||||||
|
self.state = [SIM_START_X, SIM_START_Y, SIM_START_H]
|
||||||
|
|
||||||
|
self.opt_u = np.zeros((P.M,P.T))
|
||||||
|
self.opt_u[0,:] = 1 #m/s
|
||||||
|
self.opt_u[1,:] = np.radians(0) #rad/s
|
||||||
|
|
||||||
|
# Interpolated Path to follow given waypoints
|
||||||
|
#self.path = compute_path_from_wp([0,10,12,2,4,14],[0,0,2,10,12,12])
|
||||||
|
self.path = compute_path_from_wp([0,3,4,6,10,13],
|
||||||
|
[0,0,2,4,3,3],1)
|
||||||
|
|
||||||
|
# Sim help vars
|
||||||
|
self.sim_time=0
|
||||||
|
self.x_history=[]
|
||||||
|
self.y_history=[]
|
||||||
|
self.h_history=[]
|
||||||
|
self.v_history=[]
|
||||||
|
self.w_history=[]
|
||||||
|
self.predicted=None
|
||||||
|
|
||||||
|
#Initialise plot
|
||||||
|
plt.style.use("ggplot")
|
||||||
|
self.fig = plt.figure()
|
||||||
|
plt.ion()
|
||||||
|
plt.show()
|
||||||
|
|
||||||
|
def predict_motion(self):
|
||||||
|
'''
|
||||||
|
'''
|
||||||
|
predicted=np.zeros(self.opt_u.shape)
|
||||||
|
x=self.state[0]
|
||||||
|
y=self.state[1]
|
||||||
|
th=self.state[2]
|
||||||
|
for idx,v,w in zip(range(len(self.opt_u[0,:])),self.opt_u[0,:],self.opt_u[1,:]):
|
||||||
|
x = x+v*np.cos(th)*P.dt
|
||||||
|
y = y+v*np.sin(th)*P.dt
|
||||||
|
th= th +w*P.dt
|
||||||
|
predicted[0,idx]=x
|
||||||
|
predicted[1,idx]=y
|
||||||
|
|
||||||
|
self.predicted = predicted
|
||||||
|
|
||||||
|
def run(self):
|
||||||
|
'''
|
||||||
|
'''
|
||||||
|
|
||||||
|
while 1:
|
||||||
|
if self.state is not None:
|
||||||
|
|
||||||
|
if np.sqrt((self.state[0]-self.path[0,-1])**2+(self.state[1]-self.path[1,-1])**2)<1:
|
||||||
|
print("Success! Goal Reached")
|
||||||
|
return
|
||||||
|
|
||||||
|
#optimization loop
|
||||||
|
start=time.time()
|
||||||
|
self.opt_u = optimize(self.state,
|
||||||
|
self.opt_u,
|
||||||
|
self.path)
|
||||||
|
|
||||||
|
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
|
||||||
|
|
||||||
|
self.update_sim(self.opt_u[0,1],self.opt_u[1,1])
|
||||||
|
self.predict_motion()
|
||||||
|
self.plot_sim()
|
||||||
|
|
||||||
|
def update_sim(self,lin_v,ang_v):
|
||||||
|
'''
|
||||||
|
Updates state.
|
||||||
|
|
||||||
|
:param lin_v: float
|
||||||
|
:param ang_v: float
|
||||||
|
'''
|
||||||
|
|
||||||
|
self.state[0] = self.state[0] +lin_v*np.cos(self.state[2])*P.dt
|
||||||
|
self.state[1] = self.state[1] +lin_v*np.sin(self.state[2])*P.dt
|
||||||
|
self.state[2] = self.state[2] +ang_v*P.dt
|
||||||
|
|
||||||
|
def plot_sim(self):
|
||||||
|
|
||||||
|
self.sim_time = self.sim_time+P.dt
|
||||||
|
self.x_history.append(self.state[0])
|
||||||
|
self.y_history.append(self.state[1])
|
||||||
|
self.h_history.append(self.state[2])
|
||||||
|
self.v_history.append(self.opt_u[0,1])
|
||||||
|
self.w_history.append(self.opt_u[1,1])
|
||||||
|
|
||||||
|
plt.clf()
|
||||||
|
|
||||||
|
grid = plt.GridSpec(2, 3)
|
||||||
|
|
||||||
|
plt.subplot(grid[0:2, 0:2])
|
||||||
|
plt.title("MPC Simulation \n" + "Simulation elapsed time {}s".format(self.sim_time))
|
||||||
|
|
||||||
|
plt.plot(self.path[0,:],self.path[1,:], c='tab:orange',
|
||||||
|
marker=".",
|
||||||
|
label="reference track")
|
||||||
|
|
||||||
|
plt.plot(self.x_history, self.y_history, c='tab:blue',
|
||||||
|
marker=".",
|
||||||
|
alpha=0.5,
|
||||||
|
label="vehicle trajectory")
|
||||||
|
|
||||||
|
if self.predicted is not None:
|
||||||
|
plt.plot(self.predicted[0,:], self.predicted[1,:], c='tab:green',
|
||||||
|
marker=".",
|
||||||
|
alpha=0.5,
|
||||||
|
label="mpc opt trajectory")
|
||||||
|
|
||||||
|
# plt.plot(self.x_history[-1], self.y_history[-1], c='tab:blue',
|
||||||
|
# marker=".",
|
||||||
|
# markersize=12,
|
||||||
|
# label="vehicle position")
|
||||||
|
# plt.arrow(self.x_history[-1],
|
||||||
|
# self.y_history[-1],
|
||||||
|
# np.cos(self.h_history[-1]),
|
||||||
|
# np.sin(self.h_history[-1]),
|
||||||
|
# color='tab:blue',
|
||||||
|
# width=0.2,
|
||||||
|
# head_length=0.5,
|
||||||
|
# label="heading")
|
||||||
|
|
||||||
|
plot_car(self.x_history[-1], self.y_history[-1], self.h_history[-1])
|
||||||
|
|
||||||
|
plt.ylabel('map y')
|
||||||
|
plt.yticks(np.arange(min(self.path[1,:])-1., max(self.path[1,:]+1.)+1, 1.0))
|
||||||
|
plt.xlabel('map x')
|
||||||
|
plt.xticks(np.arange(min(self.path[0,:])-1., max(self.path[0,:]+1.)+1, 1.0))
|
||||||
|
plt.axis("equal")
|
||||||
|
#plt.legend()
|
||||||
|
|
||||||
|
plt.subplot(grid[0, 2])
|
||||||
|
#plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
|
||||||
|
plt.plot(self.v_history,c='tab:orange')
|
||||||
|
locs, _ = plt.xticks()
|
||||||
|
plt.xticks(locs[1:], locs[1:]*P.dt)
|
||||||
|
plt.ylabel('v(t) [m/s]')
|
||||||
|
plt.xlabel('t [s]')
|
||||||
|
|
||||||
|
plt.subplot(grid[1, 2])
|
||||||
|
#plt.title("Angular Velocity {} m/s".format(self.w_history[-1]))
|
||||||
|
plt.plot(np.degrees(self.w_history),c='tab:orange')
|
||||||
|
plt.ylabel('w(t) [deg/s]')
|
||||||
|
locs, _ = plt.xticks()
|
||||||
|
plt.xticks(locs[1:], locs[1:]*P.dt)
|
||||||
|
plt.xlabel('t [s]')
|
||||||
|
|
||||||
|
plt.tight_layout()
|
||||||
|
|
||||||
|
plt.draw()
|
||||||
|
plt.pause(0.1)
|
||||||
|
|
||||||
|
|
||||||
|
def plot_car(x, y, yaw):
|
||||||
|
LENGTH = 1.0 # [m]
|
||||||
|
WIDTH = 0.5 # [m]
|
||||||
|
OFFSET = LENGTH/2 # [m]
|
||||||
|
|
||||||
|
outline = np.array([[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET],
|
||||||
|
[WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])
|
||||||
|
|
||||||
|
Rotm = np.array([[np.cos(yaw), np.sin(yaw)],
|
||||||
|
[-np.sin(yaw), np.cos(yaw)]])
|
||||||
|
|
||||||
|
outline = (outline.T.dot(Rotm)).T
|
||||||
|
|
||||||
|
outline[0, :] += x
|
||||||
|
outline[1, :] += y
|
||||||
|
|
||||||
|
plt.plot(np.array(outline[0, :]).flatten(), np.array(outline[1, :]).flatten(), 'tab:blue')
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def do_sim():
|
||||||
|
sim=MPC()
|
||||||
|
try:
|
||||||
|
sim.run()
|
||||||
|
except Exception as e:
|
||||||
|
sys.exit(e)
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
do_sim()
|
||||||
|
|
@ -0,0 +1,105 @@
|
||||||
|
import numpy as np
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
from matplotlib import animation
|
||||||
|
|
||||||
|
from utils import compute_path_from_wp
|
||||||
|
from cvxpy_mpc import optimize
|
||||||
|
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
|
||||||
|
import pybullet as p
|
||||||
|
import time
|
||||||
|
|
||||||
|
def get_state(robotId):
|
||||||
|
"""
|
||||||
|
"""
|
||||||
|
robPos, robOrn = p.getBasePositionAndOrientation(robotId)
|
||||||
|
linVel,angVel = p.getBaseVelocity(robotId)
|
||||||
|
|
||||||
|
return[robPos[0], robPos[1], p.getEulerFromQuaternion(robOrn)[2]]
|
||||||
|
|
||||||
|
def set_ctrl(robotId,v,w):
|
||||||
|
"""
|
||||||
|
"""
|
||||||
|
L= 0.354
|
||||||
|
R= 0.076/2
|
||||||
|
|
||||||
|
rightWheelVelocity= (2*v+w*L)/(2*R)
|
||||||
|
leftWheelVelocity = (2*v-w*L)/(2*R)
|
||||||
|
|
||||||
|
p.setJointMotorControl2(robotId,0,p.VELOCITY_CONTROL,targetVelocity=leftWheelVelocity,force=1000)
|
||||||
|
p.setJointMotorControl2(robotId,1,p.VELOCITY_CONTROL,targetVelocity=rightWheelVelocity,force=1000)
|
||||||
|
|
||||||
|
def plot(path,x_history,y_history):
|
||||||
|
"""
|
||||||
|
"""
|
||||||
|
plt.style.use("ggplot")
|
||||||
|
plt.figure()
|
||||||
|
plt.title("MPC Tracking Results")
|
||||||
|
|
||||||
|
plt.plot(path[0,:],path[1,:], c='tab:orange',marker=".",label="reference track")
|
||||||
|
plt.plot(x_history, y_history, c='tab:blue',marker=".",alpha=0.5,label="vehicle trajectory")
|
||||||
|
|
||||||
|
plt.legend()
|
||||||
|
plt.show()
|
||||||
|
|
||||||
|
def run_sim():
|
||||||
|
"""
|
||||||
|
"""
|
||||||
|
p.connect(p.GUI)
|
||||||
|
|
||||||
|
start_offset = [0,2,0]
|
||||||
|
start_orientation = p.getQuaternionFromEuler([0,0,0])
|
||||||
|
turtle = p.loadURDF("turtlebot.urdf",start_offset, start_orientation)
|
||||||
|
plane = p.loadURDF("plane.urdf")
|
||||||
|
|
||||||
|
p.setRealTimeSimulation(1)
|
||||||
|
p.setGravity(0,0,-10)
|
||||||
|
|
||||||
|
# MPC time step
|
||||||
|
dt = 0.25
|
||||||
|
|
||||||
|
# starting guess output
|
||||||
|
N = 5 #number of state variables
|
||||||
|
M = 2 #number of control variables
|
||||||
|
T = 20 #Prediction Horizon
|
||||||
|
|
||||||
|
opt_u = np.zeros((M,T))
|
||||||
|
opt_u[0,:] = 1 #m/s
|
||||||
|
opt_u[1,:] = np.radians(0) #rad/s
|
||||||
|
|
||||||
|
# Interpolated Path to follow given waypoints
|
||||||
|
path = compute_path_from_wp([0,10,12,2,4,14],[0,0,2,10,12,12])
|
||||||
|
x_history=[]
|
||||||
|
y_history=[]
|
||||||
|
|
||||||
|
while (1):
|
||||||
|
|
||||||
|
state = get_state(turtle)
|
||||||
|
x_history.append(state[0])
|
||||||
|
y_history.append(state[1])
|
||||||
|
|
||||||
|
#track path in bullet
|
||||||
|
p.addUserDebugLine([state[0],state[1],0],[state[0],state[1],0.5],[1,0,0])
|
||||||
|
|
||||||
|
if np.sqrt((state[0]-path[0,-1])**2+(state[1]-path[1,-1])**2)<1:
|
||||||
|
print("Success! Goal Reached")
|
||||||
|
set_ctrl(turtle,0,0)
|
||||||
|
plot(path,x_history,y_history)
|
||||||
|
p.disconnect()
|
||||||
|
return
|
||||||
|
|
||||||
|
#optimization loop
|
||||||
|
start=time.time()
|
||||||
|
opt_u = optimize(state,opt_u,path)
|
||||||
|
elapsed=time.time()-start
|
||||||
|
print("CVXPY Optimization Time: {:.4f}s".format(elapsed))
|
||||||
|
|
||||||
|
set_ctrl(turtle,opt_u[0,1],opt_u[1,1])
|
||||||
|
|
||||||
|
if dt-elapsed>0:
|
||||||
|
time.sleep(dt-elapsed)
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
run_sim()
|
||||||
|
|
@ -0,0 +1,84 @@
|
||||||
|
import numpy as np
|
||||||
|
from scipy.interpolate import interp1d
|
||||||
|
|
||||||
|
def compute_path_from_wp(start_xp, start_yp, step = 0.1):
|
||||||
|
"""
|
||||||
|
"""
|
||||||
|
final_xp=[]
|
||||||
|
final_yp=[]
|
||||||
|
delta = step #[m]
|
||||||
|
|
||||||
|
for idx in range(len(start_xp)-1):
|
||||||
|
section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))
|
||||||
|
|
||||||
|
interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))
|
||||||
|
|
||||||
|
fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)
|
||||||
|
fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)
|
||||||
|
|
||||||
|
final_xp=np.append(final_xp,fx(interp_range))
|
||||||
|
final_yp=np.append(final_yp,fy(interp_range))
|
||||||
|
|
||||||
|
return np.vstack((final_xp,final_yp))
|
||||||
|
|
||||||
|
def get_nn_idx(state,path):
|
||||||
|
"""
|
||||||
|
"""
|
||||||
|
dx = state[0]-path[0,:]
|
||||||
|
dy = state[1]-path[1,:]
|
||||||
|
dist = np.sqrt(dx**2 + dy**2)
|
||||||
|
nn_idx = np.argmin(dist)
|
||||||
|
|
||||||
|
try:
|
||||||
|
v = [path[0,nn_idx+1] - path[0,nn_idx],
|
||||||
|
path[1,nn_idx+1] - path[1,nn_idx]]
|
||||||
|
v /= np.linalg.norm(v)
|
||||||
|
|
||||||
|
d = [path[0,nn_idx] - state[0],
|
||||||
|
path[1,nn_idx] - state[1]]
|
||||||
|
|
||||||
|
if np.dot(d,v) > 0:
|
||||||
|
target_idx = nn_idx
|
||||||
|
else:
|
||||||
|
target_idx = nn_idx+1
|
||||||
|
|
||||||
|
except IndexError as e:
|
||||||
|
target_idx = nn_idx
|
||||||
|
|
||||||
|
return target_idx
|
||||||
|
|
||||||
|
def road_curve(state,track):
|
||||||
|
"""
|
||||||
|
"""
|
||||||
|
|
||||||
|
POLY_RANK = 3
|
||||||
|
|
||||||
|
#given vehicle pos find lookahead waypoints
|
||||||
|
nn_idx=get_nn_idx(state,track)-1
|
||||||
|
LOOKAHED = POLY_RANK + 1
|
||||||
|
lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]
|
||||||
|
|
||||||
|
#trasform lookahead waypoints to vehicle ref frame
|
||||||
|
dx = lk_wp[0,:] - state[0]
|
||||||
|
dy = lk_wp[1,:] - state[1]
|
||||||
|
|
||||||
|
wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),
|
||||||
|
dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))
|
||||||
|
|
||||||
|
#fit poly
|
||||||
|
return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], POLY_RANK, rcond=None, full=False, w=None, cov=False)
|
||||||
|
|
||||||
|
def f(x,coeff):
|
||||||
|
"""
|
||||||
|
"""
|
||||||
|
return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)
|
||||||
|
|
||||||
|
# def f(x,coeff):
|
||||||
|
# return round(coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0,6)
|
||||||
|
|
||||||
|
def df(x,coeff):
|
||||||
|
"""
|
||||||
|
"""
|
||||||
|
return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)
|
||||||
|
# def df(x,coeff):
|
||||||
|
# return round(5*coeff[0]*x**4 + 4*coeff[1]*x**3 +3*coeff[2]*x**2 + 2*coeff[3]*x**1 + coeff[4]*x**0,6)
|
||||||
File diff suppressed because one or more lines are too long
|
Before Width: | Height: | Size: 77 KiB After Width: | Height: | Size: 77 KiB |
|
Before Width: | Height: | Size: 38 KiB After Width: | Height: | Size: 38 KiB |
|
Before Width: | Height: | Size: 32 KiB After Width: | Height: | Size: 32 KiB |
Loading…
Reference in New Issue