updated location of old notebooks

master
mcarfagno 2020-10-22 21:43:47 +01:00
parent 5bb35ef95d
commit 385daab272
154 changed files with 0 additions and 7111 deletions

BIN
.DS_Store vendored

Binary file not shown.

BIN
.old/.DS_Store vendored

Binary file not shown.

View File

@ -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

View File

@ -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

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.

Before

Width:  |  Height:  |  Size: 132 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 132 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 132 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 178 KiB

File diff suppressed because one or more lines are too long

File diff suppressed because one or more lines are too long

View File

@ -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

View File

@ -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()

View File

@ -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()

View File

@ -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.

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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>

View File

@ -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: 44 KiB

View File

@ -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()

View File

@ -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()

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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>

View File

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