updated location of old notebooks
mcarfagno
parent
5bb35ef95d
commit
385daab272
Binary file not shown.
|
|
@ -1,104 +0,0 @@
|
||||||
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
|
|
||||||
|
|
@ -1,827 +0,0 @@
|
||||||
<?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.
|
Before Width: | Height: | Size: 559 KiB |
Binary file not shown.
|
Before 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.
|
Before Width: | Height: | Size: 14 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
|
Before Width: | Height: | Size: 44 KiB |
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 82 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 69 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 178 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 84 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
Before 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
|
|
@ -1,6 +0,0 @@
|
||||||
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
|
|
||||||
|
|
@ -1,207 +0,0 @@
|
||||||
#! /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()
|
|
||||||
|
|
@ -1,108 +0,0 @@
|
||||||
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
|
|
||||||
|
|
||||||
from mpc_config import Params
|
|
||||||
P=Params()
|
|
||||||
|
|
||||||
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.axis("equal")
|
|
||||||
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
|
|
||||||
P.dt = 0.25
|
|
||||||
|
|
||||||
opt_u = np.zeros((P.M,P.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,3,4,6,10,13],
|
|
||||||
[0,0,2,4,3,3],1)
|
|
||||||
|
|
||||||
for x_,y_ in zip(path[0,:],path[1,:]):
|
|
||||||
p.addUserDebugLine([x_,y_,0],[x_,y_,0.33],[0,0,1])
|
|
||||||
|
|
||||||
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 P.dt-elapsed>0:
|
|
||||||
time.sleep(P.dt-elapsed)
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
run_sim()
|
|
||||||
|
|
@ -1,84 +0,0 @@
|
||||||
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
Binary file not shown.
|
Before Width: | Height: | Size: 77 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 38 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 32 KiB |
Binary file not shown.
|
|
@ -1,178 +0,0 @@
|
||||||
import numpy as np
|
|
||||||
np.seterr(divide='ignore', invalid='ignore')
|
|
||||||
|
|
||||||
from scipy.integrate import odeint
|
|
||||||
from scipy.interpolate import interp1d
|
|
||||||
import cvxpy as cp
|
|
||||||
|
|
||||||
def get_linear_model(x_bar,u_bar):
|
|
||||||
"""
|
|
||||||
"""
|
|
||||||
|
|
||||||
# Control problem statement.
|
|
||||||
|
|
||||||
N = 5 #number of state variables
|
|
||||||
M = 2 #number of control variables
|
|
||||||
T = 20 #Prediction Horizon
|
|
||||||
dt = 0.25 #discretization step
|
|
||||||
|
|
||||||
x = x_bar[0]
|
|
||||||
y = x_bar[1]
|
|
||||||
theta = x_bar[2]
|
|
||||||
psi = x_bar[3]
|
|
||||||
cte = x_bar[4]
|
|
||||||
|
|
||||||
v = u_bar[0]
|
|
||||||
w = u_bar[1]
|
|
||||||
|
|
||||||
A = np.zeros((N,N))
|
|
||||||
A[0,2]=-v*np.sin(theta)
|
|
||||||
A[1,2]=v*np.cos(theta)
|
|
||||||
A[4,3]=v*np.cos(-psi)
|
|
||||||
A_lin=np.eye(N)+dt*A
|
|
||||||
|
|
||||||
B = np.zeros((N,M))
|
|
||||||
B[0,0]=np.cos(theta)
|
|
||||||
B[1,0]=np.sin(theta)
|
|
||||||
B[2,1]=1
|
|
||||||
B[3,1]=-1
|
|
||||||
B[4,0]=np.sin(-psi)
|
|
||||||
B_lin=dt*B
|
|
||||||
|
|
||||||
f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w,-w,v*np.sin(-psi)]).reshape(N,1)
|
|
||||||
C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))
|
|
||||||
|
|
||||||
return A_lin,B_lin,C_lin
|
|
||||||
|
|
||||||
def calc_err(state,path):
|
|
||||||
"""
|
|
||||||
Finds psi and cte w.r.t. the closest waypoint.
|
|
||||||
|
|
||||||
:param state: array_like, state of the vehicle [x_pos, y_pos, theta]
|
|
||||||
:param path: array_like, reference path ((x1, x2, ...), (y1, y2, ...), (th1 ,th2, ...)]
|
|
||||||
:returns: (float,float)
|
|
||||||
"""
|
|
||||||
|
|
||||||
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
|
|
||||||
|
|
||||||
path_ref_vect = [np.cos(path[2,target_idx] + np.pi / 2),
|
|
||||||
np.sin(path[2,target_idx] + np.pi / 2)]
|
|
||||||
|
|
||||||
#heading error w.r.t path frame
|
|
||||||
psi = path[2,target_idx] - state[2]
|
|
||||||
|
|
||||||
# the cross-track error is given by the scalar projection of the car->wp vector onto the faxle versor
|
|
||||||
#cte = np.dot([dx[target_idx], dy[target_idx]],front_axle_vect)
|
|
||||||
cte = np.dot([dx[target_idx], dy[target_idx]],path_ref_vect)
|
|
||||||
|
|
||||||
return target_idx,psi,cte
|
|
||||||
|
|
||||||
def optimize(starting_state,u_bar,track):
|
|
||||||
'''
|
|
||||||
:param starting_state:
|
|
||||||
:param u_bar:
|
|
||||||
:param track:
|
|
||||||
:returns:
|
|
||||||
'''
|
|
||||||
|
|
||||||
MAX_SPEED = 1.25
|
|
||||||
MIN_SPEED = 0.75
|
|
||||||
MAX_STEER_SPEED = 1.57/2
|
|
||||||
|
|
||||||
N = 5 #number of state variables
|
|
||||||
M = 2 #number of control variables
|
|
||||||
T = 20 #Prediction Horizon
|
|
||||||
dt = 0.25 #discretization step
|
|
||||||
|
|
||||||
#Starting Condition
|
|
||||||
x0 = np.zeros(N)
|
|
||||||
x0[0] = starting_state[0]
|
|
||||||
x0[1] = starting_state[1]
|
|
||||||
x0[2] = starting_state[2]
|
|
||||||
_,psi,cte = calc_err(x0,track)
|
|
||||||
x0[3]=psi
|
|
||||||
x0[4]=cte
|
|
||||||
|
|
||||||
# Prediction
|
|
||||||
x_bar=np.zeros((N,T+1))
|
|
||||||
x_bar[:,0]=x0
|
|
||||||
|
|
||||||
for t in range (1,T+1):
|
|
||||||
xt=x_bar[:,t-1].reshape(5,1)
|
|
||||||
ut=u_bar[:,t-1].reshape(2,1)
|
|
||||||
|
|
||||||
A,B,C=get_linear_model(xt,ut)
|
|
||||||
|
|
||||||
xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)
|
|
||||||
|
|
||||||
_,psi,cte = calc_err(xt_plus_one,track)
|
|
||||||
xt_plus_one[3]=psi
|
|
||||||
xt_plus_one[4]=cte
|
|
||||||
|
|
||||||
x_bar[:,t]= xt_plus_one
|
|
||||||
|
|
||||||
#CVXPY Linear MPC problem statement
|
|
||||||
cost = 0
|
|
||||||
constr = []
|
|
||||||
x = cp.Variable((N, T+1))
|
|
||||||
u = cp.Variable((M, T))
|
|
||||||
|
|
||||||
for t in range(T):
|
|
||||||
|
|
||||||
# Tracking
|
|
||||||
if t > 0:
|
|
||||||
idx,_,_ = calc_err(x_bar[:,t],track)
|
|
||||||
delta_x = track[:,idx]-x[0:3,t]
|
|
||||||
cost+= cp.quad_form(delta_x,10*np.eye(3))
|
|
||||||
|
|
||||||
# Tracking last time step
|
|
||||||
if t == T:
|
|
||||||
idx,_,_ = calc_err(x_bar[:,t],track)
|
|
||||||
delta_x = track[:,idx]-x[0:3,t]
|
|
||||||
cost+= cp.quad_form(delta_x,100*np.eye(3))
|
|
||||||
|
|
||||||
# Actuation rate of change
|
|
||||||
if t < (T - 1):
|
|
||||||
cost += cp.quad_form(u[:, t + 1] - u[:, t], 25*np.eye(M))
|
|
||||||
|
|
||||||
# Actuation effort
|
|
||||||
cost += cp.quad_form( u[:, t],1*np.eye(M))
|
|
||||||
|
|
||||||
# Constrains
|
|
||||||
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] == x0] # starting 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.ECOS, 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
|
|
||||||
|
|
@ -1,15 +0,0 @@
|
||||||
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
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,18 +0,0 @@
|
||||||
# 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
|
|
||||||
|
|
@ -1,29 +0,0 @@
|
||||||
<?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>
|
|
||||||
|
|
||||||
|
|
@ -1,827 +0,0 @@
|
||||||
<?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.
|
Before Width: | Height: | Size: 559 KiB |
Binary file not shown.
|
Before 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.
|
Before Width: | Height: | Size: 14 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
|
Before Width: | Height: | Size: 44 KiB |
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 82 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 69 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 178 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 84 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
Before 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
|
|
@ -1,207 +0,0 @@
|
||||||
#! /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=2
|
|
||||||
SIM_START_H=0
|
|
||||||
|
|
||||||
# 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]
|
|
||||||
# Sim step
|
|
||||||
self.dt = 0.25
|
|
||||||
|
|
||||||
# starting guess output
|
|
||||||
N = 5 #number of state variables
|
|
||||||
M = 2 #number of control variables
|
|
||||||
T = 20 #Prediction Horizon
|
|
||||||
self.opt_u = np.zeros((M,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])
|
|
||||||
|
|
||||||
# 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)*self.dt
|
|
||||||
y = y+v*np.sin(th)*self.dt
|
|
||||||
th= th +w*self.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])*self.dt
|
|
||||||
self.state[1] = self.state[1] +lin_v*np.sin(self.state[2])*self.dt
|
|
||||||
self.state[2] = self.state[2] +ang_v*self.dt
|
|
||||||
|
|
||||||
def plot_sim(self):
|
|
||||||
|
|
||||||
self.sim_time = self.sim_time+self.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.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:]*self.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:]*self.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()
|
|
||||||
|
|
@ -1,105 +0,0 @@
|
||||||
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()
|
|
||||||
|
|
@ -1,33 +0,0 @@
|
||||||
import numpy as np
|
|
||||||
from scipy.interpolate import interp1d
|
|
||||||
|
|
||||||
def compute_path_from_wp(start_xp, start_yp, step = 0.1):
|
|
||||||
"""
|
|
||||||
Interpolation range is computed to assure one point every fixed distance step [m].
|
|
||||||
|
|
||||||
:param start_xp: array_like, list of starting x coordinates
|
|
||||||
:param start_yp: array_like, list of starting y coordinates
|
|
||||||
:param step: float, interpolation distance [m] between consecutive waypoints
|
|
||||||
:returns: array_like, of shape (3,N)
|
|
||||||
"""
|
|
||||||
|
|
||||||
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,int(section_len/delta))
|
|
||||||
|
|
||||||
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))
|
|
||||||
|
|
||||||
dx = np.append(0, np.diff(final_xp))
|
|
||||||
dy = np.append(0, np.diff(final_yp))
|
|
||||||
theta = np.arctan2(dy, dx)
|
|
||||||
|
|
||||||
return np.vstack((final_xp,final_yp,theta))
|
|
||||||
Binary file not shown.
|
Before Width: | Height: | Size: 38 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 32 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 6.2 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 6.2 KiB |
|
|
@ -1,15 +0,0 @@
|
||||||
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
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,18 +0,0 @@
|
||||||
# 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
|
|
||||||
|
|
@ -1,29 +0,0 @@
|
||||||
<?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>
|
|
||||||
|
|
||||||
|
Before Width: | Height: | Size: 6.2 KiB After Width: | Height: | Size: 6.2 KiB |
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue