added pybullet simulation
mcarfagno
parent
dd63de4397
commit
f3e7ed0a84
18
README.md
18
README.md
|
|
@ -1,6 +1,6 @@
|
|||
# mpc_python
|
||||
|
||||
Python implementation of mpc controller for path tracking.
|
||||
Python implementation of a mpc controller for path tracking using **[CVXPY](https://www.cvxpy.org/)**.
|
||||
|
||||
## About
|
||||
|
||||
|
|
@ -25,12 +25,24 @@ The inputs of the model are:
|
|||
|
||||
## Demo
|
||||
|
||||
The MPC implementation is tested using **[bullet](https://pybullet.org/wordpress/)** physics simulator.
|
||||
|
||||
|
||||
|
||||
Results:
|
||||
|
||||
|
||||
|
||||
To run the demo:
|
||||
To run the pybullet demo:
|
||||
|
||||
```bash
|
||||
python3 mpc_demo/main.py
|
||||
python3 mpc_demo/mpc_demo_pybullet.py
|
||||
```
|
||||
|
||||
To run the simulation-less demo:
|
||||
|
||||
```bash
|
||||
python3 mpc_demo/mpc_demo_pybullet.py
|
||||
```
|
||||
|
||||
## Requirements
|
||||
|
|
|
|||
Binary file not shown.
|
After Width: | Height: | Size: 171 KiB |
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 6.2 KiB |
|
|
@ -0,0 +1,15 @@
|
|||
newmtl Material
|
||||
Ns 10.0000
|
||||
Ni 1.5000
|
||||
d 1.0000
|
||||
Tr 0.0000
|
||||
Tf 1.0000 1.0000 1.0000
|
||||
illum 2
|
||||
Ka 0.0000 0.0000 0.0000
|
||||
Kd 0.5880 0.5880 0.5880
|
||||
Ks 0.0000 0.0000 0.0000
|
||||
Ke 0.0000 0.0000 0.0000
|
||||
map_Ka cube.tga
|
||||
map_Kd checker_blue.png
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
# Blender v2.66 (sub 1) OBJ File: ''
|
||||
# www.blender.org
|
||||
mtllib plane.mtl
|
||||
o Plane
|
||||
v 15.000000 -15.000000 0.000000
|
||||
v 15.000000 15.000000 0.000000
|
||||
v -15.000000 15.000000 0.000000
|
||||
v -15.000000 -15.000000 0.000000
|
||||
|
||||
vt 15.000000 0.000000
|
||||
vt 15.000000 15.000000
|
||||
vt 0.000000 15.000000
|
||||
vt 0.000000 0.000000
|
||||
|
||||
usemtl Material
|
||||
s off
|
||||
f 1/1 2/2 3/3
|
||||
f 1/1 3/3 4/4
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
<?xml version="0.0" ?>
|
||||
<robot name="plane">
|
||||
<link name="planeLink">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value=".0"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="plane.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<plane normal="0 0 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
|
|
@ -0,0 +1,827 @@
|
|||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from robots/kobuki_hexagons_kinect.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<!--
|
||||
- Base : kobuki
|
||||
- Stacks : hexagons
|
||||
- 3d Sensor : kinect
|
||||
-->
|
||||
<robot name="turtlebot" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!-- new mesh -->
|
||||
<mesh filename="turtlebot/kobuki_description/meshes/main_body.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0.001 0 0.05199"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.10938" radius="0.176"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0.0 0 0.05949"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="2.4"/>
|
||||
<!-- 2.4/2.6 kg for small/big battery pack -->
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wheel_left_joint" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_left_link"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="0.00 0.115 0.0250"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<link name="wheel_left_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/kobuki_description/meshes/wheel.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.0206" radius="0.0352"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
|
||||
</inertial>
|
||||
<contact>
|
||||
<lateral_friction value="1.0"/>
|
||||
<rolling_friction value="0.0"/>
|
||||
<stiffness value="30000"/>
|
||||
<damping value="1000"/>
|
||||
</contact>
|
||||
</link>
|
||||
<joint name="wheel_right_joint" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_right_link"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="0.00 -0.115 0.0250"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<link name="wheel_right_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/kobuki_description/meshes/wheel.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.0206" radius="0.0350"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
|
||||
</inertial>
|
||||
<contact>
|
||||
<lateral_friction value="1.0"/>
|
||||
<rolling_friction value="0.0"/>
|
||||
<stiffness value="30000"/>
|
||||
<damping value="10000"/>
|
||||
</contact>
|
||||
|
||||
</link>
|
||||
<joint name="caster_front_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="caster_front_link"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="0.115 0.0 0.007"/>
|
||||
</joint>
|
||||
<link name="caster_front_link">
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.0176" radius="0.017"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
|
||||
</inertial>
|
||||
<contact>
|
||||
<lateral_friction value="0.0"/>
|
||||
<rolling_friction value="0.0"/>
|
||||
<stiffness value="30000"/>
|
||||
<damping value="10000"/>
|
||||
</contact>
|
||||
</link>
|
||||
<joint name="caster_back_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="caster_back_link"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="-0.135 0.0 0.009"/>
|
||||
</joint>
|
||||
<link name="caster_back_link">
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.0176" radius="0.017"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
|
||||
</inertial>
|
||||
<contact>
|
||||
<lateral_friction value="0.0"/>
|
||||
<rolling_friction value="0.0"/>
|
||||
<stiffness value="30000"/>
|
||||
<damping value="1000"/>
|
||||
</contact>
|
||||
</link>
|
||||
<joint name="gyro_joint" type="fixed">
|
||||
<axis xyz="0 1 0"/>
|
||||
<origin rpy="0 0 0" xyz="0.056 0.062 0.0202"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="gyro_link"/>
|
||||
</joint>
|
||||
<link name="gyro_link">
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="cliff_sensor_left_joint" type="fixed">
|
||||
<origin rpy="0 1.57079632679 0" xyz="0.08734 0.13601 0.0214"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="cliff_sensor_left_link"/>
|
||||
</joint>
|
||||
<link name="cliff_sensor_left_link">
|
||||
<inertial>
|
||||
<mass value="0.0001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="cliff_sensor_right_joint" type="fixed">
|
||||
<origin rpy="0 1.57079632679 0" xyz="0.085 -0.13601 0.0214"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="cliff_sensor_right_link"/>
|
||||
</joint>
|
||||
<link name="cliff_sensor_right_link">
|
||||
<inertial>
|
||||
<mass value="0.0001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="cliff_sensor_front_joint" type="fixed">
|
||||
<origin rpy="0 1.57079632679 0" xyz="0.156 0.00 0.0214"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="cliff_sensor_front_link"/>
|
||||
</joint>
|
||||
<link name="cliff_sensor_front_link">
|
||||
<inertial>
|
||||
<mass value="0.0001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<gazebo>
|
||||
<controller:gazebo_ros_kobuki name="kobuki_controller" plugin="libgazebo_ros_kobuki.so">
|
||||
<left_wheel_joint_name>wheel_left_joint</left_wheel_joint_name>
|
||||
<right_wheel_joint_name>wheel_right_joint</right_wheel_joint_name>
|
||||
<wheel_separation>.230</wheel_separation>
|
||||
<wheel_diameter>0.070</wheel_diameter>
|
||||
<torque>1.0</torque>
|
||||
<velocity_command_timeout>0.6</velocity_command_timeout>
|
||||
<cliff_sensor_left_name>cliff_sensor_left</cliff_sensor_left_name>
|
||||
<cliff_sensor_front_name>cliff_sensor_front</cliff_sensor_front_name>
|
||||
<cliff_sensor_right_name>cliff_sensor_right</cliff_sensor_right_name>
|
||||
<cliff_detection_threshold>0.04</cliff_detection_threshold>
|
||||
<bumper_name>bumpers</bumper_name>
|
||||
<base_collision_model_link>base_link</base_collision_model_link>
|
||||
</controller:gazebo_ros_kobuki>
|
||||
</gazebo>
|
||||
<gazebo reference="wheel_left_link">
|
||||
<mu1 value="10"/>
|
||||
<mu2 value="10"/>
|
||||
<kp value="100000000.0"/>
|
||||
<kd value="10000.0"/>
|
||||
</gazebo>
|
||||
<gazebo reference="wheel_right_link">
|
||||
<mu1 value="10"/>
|
||||
<mu2 value="10"/>
|
||||
<kp value="100000000.0"/>
|
||||
<kd value="10000.0"/>
|
||||
</gazebo>
|
||||
<gazebo reference="caster_front_link">
|
||||
<mu1 value="0"/>
|
||||
<mu2 value="0"/>
|
||||
<kp value="100000000.0"/>
|
||||
<kd value="10000.0"/>
|
||||
</gazebo>
|
||||
<gazebo reference="caster_back_link">
|
||||
<mu1 value="0"/>
|
||||
<mu2 value="0"/>
|
||||
<kp value="100000000.0"/>
|
||||
<kd value="10000.0"/>
|
||||
</gazebo>
|
||||
<gazebo reference="base_link">
|
||||
<sensor:contact name="bumpers">
|
||||
<geom>base_footprint_geom_base_link</geom>
|
||||
<topic>bumpers</topic>
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>50</updateRate>
|
||||
</sensor:contact>
|
||||
</gazebo>
|
||||
<gazebo reference="cliff_sensor_left_link">
|
||||
<sensor:ray name="cliff_sensor_left">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>50</updateRate>
|
||||
<rayCount>50</rayCount>
|
||||
<rangeCount>1</rangeCount>
|
||||
<resRange>1.0</resRange>
|
||||
<minAngle>-0.04361</minAngle>
|
||||
<maxAngle>0.04361</maxAngle>
|
||||
<minRange>0.01</minRange>
|
||||
<maxRange>0.15</maxRange>
|
||||
<displayRays>true</displayRays>
|
||||
</sensor:ray>
|
||||
</gazebo>
|
||||
<gazebo reference="cliff_sensor_right_link">
|
||||
<sensor:ray name="cliff_sensor_right">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>50</updateRate>
|
||||
<rayCount>50</rayCount>
|
||||
<rangeCount>1</rangeCount>
|
||||
<resRange>1.0</resRange>
|
||||
<minAngle>-2.5</minAngle>
|
||||
<maxAngle>2.5</maxAngle>
|
||||
<minRange>0.01</minRange>
|
||||
<maxRange>0.15</maxRange>
|
||||
<displayRays>true</displayRays>
|
||||
</sensor:ray>
|
||||
</gazebo>
|
||||
<gazebo reference="cliff_sensor_front_link">
|
||||
<sensor:ray name="cliff_sensor_front">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>50</updateRate>
|
||||
<rayCount>50</rayCount>
|
||||
<rangeCount>1</rangeCount>
|
||||
<resRange>1.0</resRange>
|
||||
<minAngle>-2.5</minAngle>
|
||||
<maxAngle>2.5</maxAngle>
|
||||
<minRange>0.01</minRange>
|
||||
<maxRange>0.15</maxRange>
|
||||
<displayRays>true</displayRays>
|
||||
</sensor:ray>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<controller:gazebo_ros_imu name="imu_controller" plugin="libgazebo_ros_imu.so">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>50</updateRate>
|
||||
<bodyName>gyro_link</bodyName>
|
||||
<topicName>/mobile_base/sensors/imu_data</topicName>
|
||||
<gaussianNoise>2.89e-06</gaussianNoise>
|
||||
<xyzOffsets>0 0 0</xyzOffsets>
|
||||
<rpyOffsets>0 0 0</rpyOffsets>
|
||||
<interface:position name="gyro_link"/>
|
||||
</controller:gazebo_ros_imu>
|
||||
</gazebo>
|
||||
<joint name="pole_bottom_0_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.12 0.082 0.1028"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_bottom_0_link"/>
|
||||
</joint>
|
||||
<link name="pole_bottom_0_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0492" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_bottom_1_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.055 0.12 0.1028"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_bottom_1_link"/>
|
||||
</joint>
|
||||
<link name="pole_bottom_1_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0492" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_bottom_2_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.055 0.12 0.1028"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_bottom_2_link"/>
|
||||
</joint>
|
||||
<link name="pole_bottom_2_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0492" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_bottom_3_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.12 -0.082 0.1028"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_bottom_3_link"/>
|
||||
</joint>
|
||||
<link name="pole_bottom_3_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0492" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_bottom_4_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.055 -0.12 0.1028"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_bottom_4_link"/>
|
||||
</joint>
|
||||
<link name="pole_bottom_4_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0492" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_bottom_5_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.055 -0.12 0.1028"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_bottom_5_link"/>
|
||||
</joint>
|
||||
<link name="pole_bottom_5_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0492" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="plate_bottom_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.02364 0.0 0.1306"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="plate_bottom_link"/>
|
||||
</joint>
|
||||
<link name="plate_bottom_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_bottom.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.26727 0.340 0.006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_middle_0_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.0381 0.1505 0.164"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_middle_0_link"/>
|
||||
</joint>
|
||||
<link name="pole_middle_0_link">
|
||||
<visual>
|
||||
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_middle.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0608" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_middle_1_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.0381 -0.1505 0.164"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_middle_1_link"/>
|
||||
</joint>
|
||||
<link name="pole_middle_1_link">
|
||||
<visual>
|
||||
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_middle.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0608" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_middle_2_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.0381 0.1505 0.164"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_middle_2_link"/>
|
||||
</joint>
|
||||
<link name="pole_middle_2_link">
|
||||
<visual>
|
||||
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_middle.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0608" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_middle_3_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.0381 -0.1505 0.164"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_middle_3_link"/>
|
||||
</joint>
|
||||
<link name="pole_middle_3_link">
|
||||
<visual>
|
||||
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_middle.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0608" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="plate_middle_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.01364 0.0 0.1874"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="plate_middle_link"/>
|
||||
</joint>
|
||||
<link name="plate_middle_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_middle.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<geometry>
|
||||
<box size="0.28727 0.340 0.006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_top_0_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.0381 0.1505 0.292"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_top_0_link"/>
|
||||
</joint>
|
||||
<link name="pole_top_0_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz=" 0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_top.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.2032" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_top_1_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.0381 -0.1505 0.292"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_top_1_link"/>
|
||||
</joint>
|
||||
<link name="pole_top_1_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz=" 0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_top.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.2032" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_top_2_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.0381 0.1505 0.292"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_top_2_link"/>
|
||||
</joint>
|
||||
<link name="pole_top_2_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz=" 0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_top.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.2032" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_top_3_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.0381 -0.1505 0.292"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_top_3_link"/>
|
||||
</joint>
|
||||
<link name="pole_top_3_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz=" 0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_top.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.2032" radius=".006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_kinect_0_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.1024 0.098 0.2372"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_kinect_0_link"/>
|
||||
</joint>
|
||||
<link name="pole_kinect_0_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<!-- <origin xyz="0 0 0" rpy="${-M_PI/2} ${M_PI} 0"/> -->
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_kinect.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0936" radius="0.006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="pole_kinect_1_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.1024 -0.098 0.2372"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="pole_kinect_1_link"/>
|
||||
</joint>
|
||||
<link name="pole_kinect_1_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<!-- <origin xyz="0 0 0" rpy="${-M_PI/2} ${M_PI} 0"/> -->
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_kinect.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0936" radius="0.006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="plate_top_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.01364 0.0 0.3966"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="plate_top_link"/>
|
||||
</joint>
|
||||
<link name="plate_top_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_top.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.28727 0.340 0.006"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="camera_rgb_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.087 -0.0125 0.287"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="camera_rgb_frame"/>
|
||||
</joint>
|
||||
<link name="camera_rgb_frame">
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="camera_rgb_optical_joint" type="fixed">
|
||||
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
|
||||
<parent link="camera_rgb_frame"/>
|
||||
<child link="camera_rgb_optical_frame"/>
|
||||
</joint>
|
||||
<link name="camera_rgb_optical_frame">
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.031 0.0125 -0.016"/>
|
||||
<parent link="camera_rgb_frame"/>
|
||||
<child link="eyes"/>
|
||||
</joint>
|
||||
<link name="eyes">
|
||||
<visual>
|
||||
<origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="turtlebot/turtlebot_description/meshes/sensors/kinect.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.0315 0.0 -0.017"/>
|
||||
<geometry>
|
||||
<box size="0.07271 0.27794 0.073"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="camera_depth_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.025 0"/>
|
||||
<parent link="camera_rgb_frame"/>
|
||||
<child link="camera_depth_frame"/>
|
||||
</joint>
|
||||
<link name="camera_depth_frame">
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="camera_depth_optical_joint" type="fixed">
|
||||
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
|
||||
<parent link="camera_depth_frame"/>
|
||||
<child link="camera_depth_optical_frame"/>
|
||||
</joint>
|
||||
<link name="camera_depth_optical_frame">
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
</robot>
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 559 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 92 KiB |
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 14 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 44 KiB |
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 82 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 69 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 178 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 84 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 132 KiB |
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
|
|
@ -0,0 +1,102 @@
|
|||
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 do_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)
|
||||
|
||||
state = get_state(turtle)
|
||||
|
||||
# MPC 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__':
|
||||
do_sim()
|
||||
|
|
@ -1,3 +1,4 @@
|
|||
pybullet==2.7.2
|
||||
cvxpy==1.0.28
|
||||
numpy==1.18.1
|
||||
osqp==0.6.1
|
||||
|
|
|
|||
Loading…
Reference in New Issue