mirror of https://github.com/ArduPilot/ardupilot
SITL: added robot.py pybullet example code
This commit is contained in:
Andrew Tridgell
parent
86d53c96d1
commit
5d9e0cb58d
|
|
@ -0,0 +1,3 @@
|
|||
This is based on the iris from here:
|
||||
https://github.com/ethz-asl/rotors_simulator/blob/master/rotors_description/urdf/iris.xacro
|
||||
converted to a urdf using xacro in ROS
|
||||
|
|
@ -0,0 +1,258 @@
|
|||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from /home/tridge/project/UAV/pybullet/rotors_simulator/rotors_description/urdf/iris.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="iris">
|
||||
<link name="iris/base_link"/>
|
||||
<joint name="iris/base_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="iris/base_link"/>
|
||||
<child link="iris/base_link_inertia"/>
|
||||
</joint>
|
||||
<link name="iris/base_link_inertia">
|
||||
<inertial>
|
||||
<mass value="1.5"/>
|
||||
<!-- [kg] -->
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.0347563" ixy="0.0" ixz="0.0" iyy="0.0458929" iyz="0.0" izz="0.0977"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iris.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0 0 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.47 0.47 0.11"/>
|
||||
<!-- [m] [m] [m] -->
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<!-- attach multirotor_base_plugin to the base_link -->
|
||||
<gazebo>
|
||||
<plugin filename="librotors_gazebo_multirotor_base_plugin.so" name="multirotor_base_plugin">
|
||||
<robotNamespace>iris</robotNamespace>
|
||||
<linkName>iris/base_link</linkName>
|
||||
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<joint name="iris/rotor_0_joint" type="continuous">
|
||||
<origin rpy="0 0 0" xyz="0.13 -0.22 0.023"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!-- TODO(ff): not currently set because it's not yet supported -->
|
||||
<!-- <limit effort="2000" velocity="${max_rot_velocity}" /> -->
|
||||
<parent link="iris/base_link"/>
|
||||
<child link="iris/rotor_0"/>
|
||||
</joint>
|
||||
<link name="iris/rotor_0">
|
||||
<inertial>
|
||||
<mass value="0.00005"/>
|
||||
<!-- [kg] -->
|
||||
<inertia ixx="9.7499961e-07" ixy="0.0" ixz="0.0" iyy="4.170414998500001e-05" iyz="0.0" izz="4.2604149625000006e-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/propeller_ccw.dae" scale="0.1 0.1 0.1"/>
|
||||
<!-- The propeller meshes have a radius of 1m -->
|
||||
<!-- <box size="${2*radius_rotor} 0.01 0.005"/> -->
|
||||
</geometry>
|
||||
<material name="green">
|
||||
<color rgba="0 1 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.005" radius="0.1"/>
|
||||
<!-- [m] -->
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo>
|
||||
<plugin filename="librotors_gazebo_motor_model.so" name="iris_front_right_motor_model">
|
||||
<robotNamespace>iris</robotNamespace>
|
||||
<jointName>iris/rotor_0_joint</jointName>
|
||||
<linkName>iris/rotor_0</linkName>
|
||||
<turningDirection>ccw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>838</maxRotVelocity>
|
||||
<motorConstant>8.54858e-06</motorConstant>
|
||||
<momentConstant>0.016</momentConstant>
|
||||
<commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
|
||||
<motorNumber>0</motorNumber>
|
||||
<rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
|
||||
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<gazebo reference="iris/rotor_0">
|
||||
<material>Gazebo/Blue</material>
|
||||
</gazebo>
|
||||
<joint name="iris/rotor_1_joint" type="continuous">
|
||||
<origin rpy="0 0 0" xyz="-0.13 0.2 0.023"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!-- TODO(ff): not currently set because it's not yet supported -->
|
||||
<!-- <limit effort="2000" velocity="${max_rot_velocity}" /> -->
|
||||
<parent link="iris/base_link"/>
|
||||
<child link="iris/rotor_1"/>
|
||||
</joint>
|
||||
<link name="iris/rotor_1">
|
||||
<inertial>
|
||||
<mass value="0.00005"/>
|
||||
<!-- [kg] -->
|
||||
<inertia ixx="9.7499961e-07" ixy="0.0" ixz="0.0" iyy="4.170414998500001e-05" iyz="0.0" izz="4.2604149625000006e-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/propeller_ccw.dae" scale="0.1 0.1 0.1"/>
|
||||
<!-- The propeller meshes have a radius of 1m -->
|
||||
<!-- <box size="${2*radius_rotor} 0.01 0.005"/> -->
|
||||
</geometry>
|
||||
<material name="green">
|
||||
<color rgba="0 1 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.005" radius="0.1"/>
|
||||
<!-- [m] -->
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo>
|
||||
<plugin filename="librotors_gazebo_motor_model.so" name="iris_back_left_motor_model">
|
||||
<robotNamespace>iris</robotNamespace>
|
||||
<jointName>iris/rotor_1_joint</jointName>
|
||||
<linkName>iris/rotor_1</linkName>
|
||||
<turningDirection>ccw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>838</maxRotVelocity>
|
||||
<motorConstant>8.54858e-06</motorConstant>
|
||||
<momentConstant>0.016</momentConstant>
|
||||
<commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
|
||||
<motorNumber>1</motorNumber>
|
||||
<rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
|
||||
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<gazebo reference="iris/rotor_1">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
<joint name="iris/rotor_2_joint" type="continuous">
|
||||
<origin rpy="0 0 0" xyz="0.13 0.22 0.023"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!-- TODO(ff): not currently set because it's not yet supported -->
|
||||
<!-- <limit effort="2000" velocity="${max_rot_velocity}" /> -->
|
||||
<parent link="iris/base_link"/>
|
||||
<child link="iris/rotor_2"/>
|
||||
</joint>
|
||||
<link name="iris/rotor_2">
|
||||
<inertial>
|
||||
<mass value="0.00005"/>
|
||||
<!-- [kg] -->
|
||||
<inertia ixx="9.7499961e-07" ixy="0.0" ixz="0.0" iyy="4.170414998500001e-05" iyz="0.0" izz="4.2604149625000006e-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/propeller_cw.dae" scale="0.1 0.1 0.1"/>
|
||||
<!-- The propeller meshes have a radius of 1m -->
|
||||
<!-- <box size="${2*radius_rotor} 0.01 0.005"/> -->
|
||||
</geometry>
|
||||
<material name="red">
|
||||
<color rgba="1 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.005" radius="0.1"/>
|
||||
<!-- [m] -->
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo>
|
||||
<plugin filename="librotors_gazebo_motor_model.so" name="iris_front_left_motor_model">
|
||||
<robotNamespace>iris</robotNamespace>
|
||||
<jointName>iris/rotor_2_joint</jointName>
|
||||
<linkName>iris/rotor_2</linkName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>838</maxRotVelocity>
|
||||
<motorConstant>8.54858e-06</motorConstant>
|
||||
<momentConstant>0.016</momentConstant>
|
||||
<commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
|
||||
<motorNumber>2</motorNumber>
|
||||
<rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
|
||||
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<gazebo reference="iris/rotor_2">
|
||||
<material>Gazebo/Blue</material>
|
||||
</gazebo>
|
||||
<joint name="iris/rotor_3_joint" type="continuous">
|
||||
<origin rpy="0 0 0" xyz="-0.13 -0.2 0.023"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!-- TODO(ff): not currently set because it's not yet supported -->
|
||||
<!-- <limit effort="2000" velocity="${max_rot_velocity}" /> -->
|
||||
<parent link="iris/base_link"/>
|
||||
<child link="iris/rotor_3"/>
|
||||
</joint>
|
||||
<link name="iris/rotor_3">
|
||||
<inertial>
|
||||
<mass value="0.00005"/>
|
||||
<!-- [kg] -->
|
||||
<inertia ixx="9.7499961e-07" ixy="0.0" ixz="0.0" iyy="4.170414998500001e-05" iyz="0.0" izz="4.2604149625000006e-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/propeller_cw.dae" scale="0.1 0.1 0.1"/>
|
||||
<!-- The propeller meshes have a radius of 1m -->
|
||||
<!-- <box size="${2*radius_rotor} 0.01 0.005"/> -->
|
||||
</geometry>
|
||||
<material name="red">
|
||||
<color rgba="1 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.005" radius="0.1"/>
|
||||
<!-- [m] -->
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo>
|
||||
<plugin filename="librotors_gazebo_motor_model.so" name="iris_back_right_motor_model">
|
||||
<robotNamespace>iris</robotNamespace>
|
||||
<jointName>iris/rotor_3_joint</jointName>
|
||||
<linkName>iris/rotor_3</linkName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>838</maxRotVelocity>
|
||||
<motorConstant>8.54858e-06</motorConstant>
|
||||
<momentConstant>0.016</momentConstant>
|
||||
<commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
|
||||
<motorNumber>3</motorNumber>
|
||||
<rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>
|
||||
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<gazebo reference="iris/rotor_3">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
</robot>
|
||||
|
||||
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,2 @@
|
|||
This is based on the opendog from here:
|
||||
https://github.com/robotlearn/pyrobolearn
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,660 @@
|
|||
<?xml version="1.0" ?>
|
||||
<robot name="opendog">
|
||||
|
||||
<gazebo>
|
||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
||||
<robotNamespace>/opendog</robotNamespace>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin xyz="0.0146925753707255 -2.32783997689001E-07 -0.0620003566767632" rpy="0 0 0" />
|
||||
<mass value="7.50050303907628" />
|
||||
<inertia
|
||||
ixx="0.0329067626307138"
|
||||
ixy="0.0145256752484318"
|
||||
ixz="-0.00262898937356677"
|
||||
iyy="0.324694201481025"
|
||||
iyz="8.6440436464913E-08"
|
||||
izz="0.3401479131296" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material name="base">
|
||||
<color rgba="0.79 0.2 0.93 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name="lf_hip">
|
||||
<inertial>
|
||||
<origin xyz="0.206139391007352 0.0113727026365967 -0.0186282798260052" rpy="0 0 0" />
|
||||
<mass value="1.29407279885193" />
|
||||
<inertia
|
||||
ixx="0.00300672363695354"
|
||||
ixy="0.000279956457563162"
|
||||
ixz="0.00219380954236559"
|
||||
iyy="0.00473624583525706"
|
||||
iyz="-0.000278492304960671"
|
||||
izz="0.00444577132964834" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/lf_hip.STL" />
|
||||
</geometry>
|
||||
<material name="dred">
|
||||
<color rgba="0.2 0.0 0.0 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/lf_hip.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="lf_hip_joint" type="revolute">
|
||||
<origin
|
||||
xyz="0.12725 0.11015 -0.0402500002980233"
|
||||
rpy="-0.0312716864214877 -2.31194431024223E-16 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="lf_hip" />
|
||||
<axis xyz="1 0 0" />
|
||||
<limit effort="30" velocity="1.0" lower="-0.2" upper="0.2" />
|
||||
<dynamics damping="0.7" friction="1.0" />
|
||||
</joint>
|
||||
|
||||
<link name="lf_upperleg">
|
||||
<inertial>
|
||||
<origin xyz="-0.14026071186531 -0.145897350328308 -0.0866287296834724" rpy="0 0 0" />
|
||||
<mass value="1.99709298869724" />
|
||||
<inertia
|
||||
ixx="0.0178522819140482"
|
||||
ixy="-0.000417486136867743"
|
||||
ixz="-0.000174817808114832"
|
||||
iyy="0.00725617205054363"
|
||||
iyz="0.000753747543548694"
|
||||
izz="0.0140200357809471" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/lf_upperleg.STL" />
|
||||
</geometry>
|
||||
<material name="dgreen">
|
||||
<color rgba="0.0 0.4 0.0 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/lf_upperleg.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="lf_upperleg_joint" type="revolute">
|
||||
<origin xyz="0.1875 -0.0067843 0" rpy="0.53639 5.5511E-16 -1.5708" />
|
||||
<parent link="lf_hip" />
|
||||
<child link="lf_upperleg" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<dynamics damping="1.0" friction="1.0"/>
|
||||
<limit effort="30" velocity="1.0" lower="-0.2" upper="0.2" />
|
||||
<dynamics damping="0.7" friction="1.0" />
|
||||
</joint>
|
||||
|
||||
<link name="lf_lowerleg">
|
||||
<inertial>
|
||||
<origin xyz="-0.0841961666836097 -0.111586779603519 -0.103991209798406" rpy="0 0 0" />
|
||||
<mass value="0.517872437395086" />
|
||||
<inertia
|
||||
ixx="0.00480929241655381"
|
||||
ixy="3.22137831183176E-05"
|
||||
ixz="2.82581039098233E-05"
|
||||
iyy="0.00230896942537742"
|
||||
iyz="-0.00238100033014549"
|
||||
izz="0.00250151457928357" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/lf_lowerleg.STL" />
|
||||
</geometry>
|
||||
<material name="dblue">
|
||||
<color rgba="0.0 0.0 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/lf_lowerleg.STL" />
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100.0</mu>
|
||||
<mu2>100.0</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="lf_lowerleg_joint" type="revolute">
|
||||
<origin xyz="-0.036327 -0.31067 -0.17972" rpy="1.2469 -6.6613E-16 1.36E-15" />
|
||||
<parent link="lf_upperleg" />
|
||||
<child link="lf_lowerleg" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="30" velocity="1.0" lower="-0.2" upper="0.2" />
|
||||
<dynamics damping="0.7" friction="1.0" />
|
||||
</joint>
|
||||
|
||||
<link name="rf_hip">
|
||||
<inertial>
|
||||
<origin xyz="0.147048151306843 -0.0117894282365432 -0.0186949790750277" rpy="0 0 0" />
|
||||
<mass value="1.31260778257106" />
|
||||
<inertia
|
||||
ixx="0.00297889838902065"
|
||||
ixy="0.000632756157402181"
|
||||
ixz="-0.00237042295251855"
|
||||
iyy="0.00528682887814262"
|
||||
iyz="0.00027007218401832"
|
||||
izz="0.00504189542831131" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/rf_hip.STL" />
|
||||
</geometry>
|
||||
<material name="dred">
|
||||
<color rgba="0.2 0.0 0.0 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/rf_hip.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="rf_hip_joint" type="revolute">
|
||||
<origin xyz="0.12725 -0.11015 -0.04025" rpy="-0.0024962 -2.3119E-16 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="rf_hip" />
|
||||
<axis xyz="1 0 0" />
|
||||
<limit effort="30" velocity="1.0" lower="-0.2" upper="0.2" />
|
||||
<dynamics damping="0.7" friction="1.0" />
|
||||
</joint>
|
||||
|
||||
<link name="rf_upperleg">
|
||||
<inertial>
|
||||
<origin xyz="-0.133628467398785 -0.162488031463387 0.0486169366921988" rpy="0 0 0" />
|
||||
<mass value="1.97849427273776" />
|
||||
<inertia
|
||||
ixx="0.0177862556152254"
|
||||
ixy="-0.000449816128990764"
|
||||
ixz="6.84609806285059E-05"
|
||||
iyy="0.0079887226695027"
|
||||
iyz="-0.00227974326372488"
|
||||
izz="0.0131967613985927" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/rf_upperleg.STL" />
|
||||
</geometry>
|
||||
<material name="dgreen">
|
||||
<color rgba="0.0 0.4 0.0 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/rf_upperleg.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="rf_upperleg_joint" type="revolute">
|
||||
<origin xyz="0.1875 0 0" rpy="2.37324114934713 1.94289029309402E-16 1.5707963267949" />
|
||||
<parent link="rf_hip" />
|
||||
<child link="rf_upperleg" />
|
||||
<axis xyz="1 0 0" />
|
||||
<limit effort="30" velocity="1.0" lower="-0.2" upper="0.2" />
|
||||
<dynamics damping="0.7" friction="1.0" />
|
||||
</joint>
|
||||
|
||||
<link name="rf_lowerleg">
|
||||
<inertial>
|
||||
<origin xyz="-0.0942678335224335 -0.11438115082609 0.100958934418855" rpy="0 0 0" />
|
||||
<mass value="0.517767036542794" />
|
||||
<inertia
|
||||
ixx="0.00480685472285392"
|
||||
ixy="3.29278427633523E-05"
|
||||
ixz="-2.73356037554584E-05"
|
||||
iyy="0.00217881170361675"
|
||||
iyz="0.00237102005421024"
|
||||
izz="0.00262923241964085" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/rf_lowerleg.STL" />
|
||||
</geometry>
|
||||
<material name="dblue">
|
||||
<color rgba="0.0 0.0 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/rf_lowerleg.STL" />
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100.0</mu>
|
||||
<mu2>100.0</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="rf_lowerleg_joint" type="revolute">
|
||||
<origin xyz="-0.0194699194772098 -0.3449998 0.0989375976332662" rpy="-1.05391222064566 1.11022302462516E-16 4.44089209850063E-16" />
|
||||
<parent link="rf_upperleg" />
|
||||
<child link="rf_lowerleg" />
|
||||
<axis xyz="1 0 0" />
|
||||
<limit effort="30" velocity="1.0" lower="-0.2" upper="0.2" />
|
||||
<dynamics damping="0.7" friction="1.0" />
|
||||
</joint>
|
||||
|
||||
<link name="lb_hip">
|
||||
<inertial>
|
||||
<origin xyz="-0.1388606089949 0.0113727026302812 -0.0186282798249506" rpy="0 0 0" />
|
||||
<mass value="1.2940727989479" />
|
||||
<inertia
|
||||
ixx="0.00300672363731876"
|
||||
ixy="0.000279956457431885"
|
||||
ixz="0.00219380954241839"
|
||||
iyy="0.00473624583534201"
|
||||
iyz="-0.000278492304808429"
|
||||
izz="0.00444577133007972" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/lb_hip.STL" />
|
||||
</geometry>
|
||||
<material name="dred">
|
||||
<color rgba="0.2 0.0 0.0 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/lb_hip.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="lb_hip_joint" type="revolute">
|
||||
<origin xyz="-0.09275 0.11015 -0.04025" rpy="-0.013144 -2.3119E-16 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="lb_hip" />
|
||||
<axis xyz="1 0 0" />
|
||||
<limit effort="30" velocity="1.0" lower="-0.2" upper="0.2" />
|
||||
<dynamics damping="0.7" friction="1.0" />
|
||||
</joint>
|
||||
|
||||
<link name="lb_upperleg">
|
||||
<inertial>
|
||||
<origin xyz="-0.155362482214265 -0.162561779303306 -0.0486234661189762" rpy="0 0 0" />
|
||||
<mass value="1.99709298565448" />
|
||||
<inertia
|
||||
ixx="0.0178522818686579"
|
||||
ixy="-0.000447434633476266"
|
||||
ixz="-6.8250948714299E-05"
|
||||
iyy="0.00800967976641392"
|
||||
iyz="0.00225765032495757"
|
||||
izz="0.0132665280240321" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/lb_upperleg.STL" />
|
||||
</geometry>
|
||||
<material name="dgreen">
|
||||
<color rgba="0.0 0.4 0.0 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/lb_upperleg.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="lb_upperleg_joint" type="revolute">
|
||||
<origin xyz="-0.1575 -0.0218860343925495 0" rpy="0.853532347704987 1.69309011255336E-15 -1.5707963267949" />
|
||||
<parent link="lb_hip" />
|
||||
<child link="lb_upperleg" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="30" velocity="1.0" lower="-0.2" upper="0.2" />
|
||||
<dynamics damping="0.7" friction="1.0" />
|
||||
</joint>
|
||||
|
||||
<link name="lb_lowerleg">
|
||||
<inertial>
|
||||
<origin xyz="-0.0929078835580175 -0.122424353782922 -0.0909838234681657" rpy="0 0 0" />
|
||||
<mass value="0.51787240839959" />
|
||||
<inertia
|
||||
ixx="0.00480929178404235"
|
||||
ixy="3.51469446486784E-05"
|
||||
ixz="2.45137702500061E-05"
|
||||
iyy="0.00178682948739438"
|
||||
iyz="-0.00230130305704765"
|
||||
izz="0.00302365388096736" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/lb_lowerleg.STL" />
|
||||
</geometry>
|
||||
<material name="dblue">
|
||||
<color rgba="0.0 0.0 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/lb_lowerleg.STL" />
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100.0</mu>
|
||||
<mu2>100.0</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="lb_lowerleg_joint" type="revolute">
|
||||
<origin xyz="-0.0427168001472368 -0.3449998 -0.0989375976332639" rpy="1.02808696809058 1.17961196366423E-15 0" />
|
||||
<parent link="lb_upperleg" />
|
||||
<child link="lb_lowerleg" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="30" velocity="1.0" lower="-0.2" upper="0.2" />
|
||||
<dynamics damping="0.7" friction="1.0" />
|
||||
</joint>
|
||||
|
||||
<link name="rb_hip">
|
||||
<inertial>
|
||||
<origin xyz="-0.197951848727426 -0.011789428219475 -0.0186949790745794" rpy="0 0 0" />
|
||||
<mass value="1.31260778161674" />
|
||||
<inertia
|
||||
ixx="0.00297889838847175"
|
||||
ixy="0.000632756156394369"
|
||||
ixz="-0.00237042295242676"
|
||||
iyy="0.0052868288763741"
|
||||
iyz="0.000270072183992474"
|
||||
izz="0.00504189542537898" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/rb_hip.STL" />
|
||||
</geometry>
|
||||
<material name="dred">
|
||||
<color rgba="0.2 0.0 0.0 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/rb_hip.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="rb_hip_joint" type="revolute">
|
||||
<origin xyz="-0.09275 -0.11015 -0.0402500002980231" rpy="0.0264641395628229 -2.31194431024223E-16 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="rb_hip" />
|
||||
<axis xyz="1 0 0" />
|
||||
<limit effort="30" velocity="1.0" lower="-0.2" upper="0.2" />
|
||||
<dynamics damping="0.7" friction="1.0" />
|
||||
</joint>
|
||||
|
||||
<link name="rb_upperleg">
|
||||
<inertial>
|
||||
<origin xyz="-0.133628467244866 -0.162488031755644 0.0486169350227676" rpy="0 0 0" />
|
||||
<mass value="1.97849425051924" />
|
||||
<inertia
|
||||
ixx="0.0177862553816508"
|
||||
ixy="-0.000449816152751435"
|
||||
ixz="6.84610936050734E-05"
|
||||
iyy="0.00798872258305596"
|
||||
iyz="-0.00227974318574354"
|
||||
izz="0.0131967612321745" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/rb_upperleg.STL" />
|
||||
</geometry>
|
||||
<material name="dgreen">
|
||||
<color rgba="0.0 0.4 0.0 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/rb_upperleg.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="rb_upperleg_joint" type="revolute">
|
||||
<origin xyz="-0.1575 0 0" rpy="2.21660126749357 1.97064586870965E-15 1.5707963267949" />
|
||||
<parent link="rb_hip" />
|
||||
<child link="rb_upperleg" />
|
||||
<axis xyz="1 0 0" />
|
||||
<limit effort="30" velocity="1.0" lower="-0.2" upper="0.2" />
|
||||
<dynamics damping="0.7" friction="1.0" />
|
||||
</joint>
|
||||
|
||||
<link name="rb_lowerleg">
|
||||
<inertial>
|
||||
<origin xyz="-0.106105013771209 -0.121605431885754 0.092129655679019" rpy="0 0 0" />
|
||||
<mass value="0.517767043602124" />
|
||||
<inertia
|
||||
ixx="0.00480685478586608"
|
||||
ixy="3.48785418068089E-05"
|
||||
ixz="-2.47987883971276E-05"
|
||||
iyy="0.00182797294270112"
|
||||
iyz="0.00231097886469285"
|
||||
izz="0.00298007124712522" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/rb_lowerleg.STL" />
|
||||
</geometry>
|
||||
<material name="dblue">
|
||||
<color rgba="0.0 0.0 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/rb_lowerleg.STL" />
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100.0</mu>
|
||||
<mu2>100.0</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="rb_lowerleg_joint" type="revolute">
|
||||
<origin xyz="-0.00763273689589838 -0.344999799999999 0.0989375976332641" rpy="-0.978531986560699 -1.70696790036118E-15 -1.22124532708767E-15" />
|
||||
<parent link="rb_upperleg" />
|
||||
<child link="rb_lowerleg" />
|
||||
<axis xyz="1 0 0" />
|
||||
<limit effort="30" velocity="1.0" lower="-0.2" upper="0.2" />
|
||||
<dynamics damping="0.7" friction="1.0" />
|
||||
</joint>
|
||||
|
||||
<!-- Transmissions for gazebo implementation -->
|
||||
<transmission name="lb_hip_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lb_hip_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="lb_hip_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="lb_upperleg_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lb_upperleg_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="lb_upperleg_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="lb_lowerleg_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lb_lowerleg_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="lb_lowerleg_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="lf_hip_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lf_hip_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="lf_hip_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="lf_upperleg_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lf_upperleg_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="lf_upperleg_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="lf_lowerleg_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lf_lowerleg_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="lf_lowerleg_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="rb_hip_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="rb_hip_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="rb_hip_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="rb_upperleg_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="rb_upperleg_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="rb_upperleg_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="rb_lowerleg_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="rb_lowerleg_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="rb_lowerleg_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="rf_hip_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="rf_hip_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="rf_hip_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="rf_upperleg_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="rf_upperleg_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="rf_upperleg_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="rf_lowerleg_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="rf_lowerleg_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="rf_lowerleg_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,330 @@
|
|||
#!/usr/bin/env python3
|
||||
'''
|
||||
example rover for JSON backend using pybullet
|
||||
based on racecar example from pybullet
|
||||
'''
|
||||
|
||||
import os, inspect, sys
|
||||
|
||||
import socket
|
||||
import struct
|
||||
import json
|
||||
import math
|
||||
|
||||
from pyrobolearn.simulators import Bullet
|
||||
import pybullet_data
|
||||
|
||||
# use pymavlink for ArduPilot convention transformations
|
||||
from pymavlink.rotmat import Vector3, Matrix3
|
||||
from pymavlink.quaternion import Quaternion
|
||||
from pyrobolearn.utils.transformation import get_rpy_from_quaternion
|
||||
import pyrobolearn as prl
|
||||
|
||||
import time
|
||||
|
||||
import argparse
|
||||
from math import degrees, radians
|
||||
|
||||
parser = argparse.ArgumentParser(description="pybullet robot")
|
||||
parser.add_argument("--vehicle", required=True, choices=['quad', 'racecar', 'iris', 'opendog', 'all'], default='iris', help="vehicle type")
|
||||
parser.add_argument("--fps", type=float, default=1000.0, help="physics frame rate")
|
||||
parser.add_argument("--stadium", default=False, action='store_true', help="use stadium for world")
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
RATE_HZ = args.fps
|
||||
TIME_STEP = 1.0 / RATE_HZ
|
||||
GRAVITY_MSS = 9.80665
|
||||
|
||||
# Create simulator
|
||||
sim = Bullet()
|
||||
|
||||
# create world
|
||||
from pyrobolearn.worlds import BasicWorld
|
||||
world = BasicWorld(sim)
|
||||
|
||||
if args.stadium:
|
||||
world.sim.remove_body(world.floor_id)
|
||||
world.floor_id = world.sim.load_sdf(os.path.join(pybullet_data.getDataPath(), "stadium.sdf"), position=[0,0,0])
|
||||
|
||||
# setup keyboard interface
|
||||
interface = prl.tools.interfaces.MouseKeyboardInterface()
|
||||
|
||||
def control_quad(pwm):
|
||||
'''control quadcopter'''
|
||||
motor_dir = [ 1, 1, -1, -1 ]
|
||||
motor_order = [ 0, 1, 2, 3 ]
|
||||
|
||||
motors = pwm[0:4]
|
||||
motor_speed = [ 0 ] * 4
|
||||
for m in range(len(motors)):
|
||||
motor_speed[motor_order[m]] = constrain(motors[m] - 1000.0, 0, 1000) * motor_dir[motor_order[m]]
|
||||
|
||||
robot.set_propeller_velocities(motor_speed)
|
||||
|
||||
def control_racecar(pwm):
|
||||
'''control racecar'''
|
||||
steer_max = 45.0
|
||||
throttle_max = 200.0
|
||||
steering = constrain((pwm[0] - 1500.0)/500.0, -1, 1) * math.radians(steer_max) * -1
|
||||
throttle = constrain((pwm[2] - 1500.0)/500.0, -1, 1) * throttle_max
|
||||
|
||||
robot.steer(steering)
|
||||
robot.drive(throttle)
|
||||
|
||||
last_angles = [0.0] * 12
|
||||
|
||||
def control_joints(pwm):
|
||||
'''control a joint based bot'''
|
||||
global last_angles
|
||||
max_angle = radians(90)
|
||||
joint_speed = radians(30)
|
||||
pwm = pwm[0:len(robot.joints)]
|
||||
angles = [ constrain((v-1500.0)/500.0, -1, 1) * max_angle for v in pwm ]
|
||||
current = last_angles
|
||||
max_change = joint_speed * TIME_STEP
|
||||
for i in range(len(angles)):
|
||||
angles[i] = constrain(angles[i], current[i]-max_change, current[i]+max_change)
|
||||
robot.set_joint_positions(angles, robot.joints)
|
||||
last_angles = angles
|
||||
|
||||
|
||||
if args.vehicle == 'iris':
|
||||
from pyrobolearn.robots import Quadcopter
|
||||
robot = Quadcopter(sim, urdf="models/iris/iris.urdf")
|
||||
control_pwm = control_quad
|
||||
elif args.vehicle == 'racecar':
|
||||
from pyrobolearn.robots import F10Racecar
|
||||
robot = F10Racecar(sim)
|
||||
control_pwm = control_racecar
|
||||
elif args.vehicle == 'opendog':
|
||||
from pyrobolearn.robots import OpenDog
|
||||
robot = OpenDog(sim, urdf="models/opendog/opendog.urdf")
|
||||
control_pwm = control_joints
|
||||
elif args.vehicle == 'all':
|
||||
from pyrobolearn.robots import OpenDog, Aibo, Ant, ANYmal, HyQ, HyQ2Max, Laikago, LittleDog, Minitaur, Pleurobot, Crab, Morphex, Rhex, SEAHexapod
|
||||
bots = [Crab, Morphex, Rhex, SEAHexapod, Aibo, Ant, ANYmal, HyQ, HyQ2Max, Laikago, LittleDog, Minitaur, Pleurobot ]
|
||||
for i in range(len(bots)):
|
||||
r = bots[i](sim)
|
||||
r.position = [0, i*2, 2]
|
||||
control_pwm = control_joints
|
||||
robot = OpenDog(sim, urdf="models/opendog/opendog.urdf")
|
||||
else:
|
||||
raise Exception("Bad vehicle")
|
||||
|
||||
|
||||
sim.set_time_step(TIME_STEP)
|
||||
|
||||
time_now = 0
|
||||
last_velocity = None
|
||||
|
||||
def quaternion_to_AP(quaternion):
|
||||
'''convert pybullet quaternion to ArduPilot quaternion'''
|
||||
return Quaternion([quaternion[3], quaternion[0], -quaternion[1], -quaternion[2]])
|
||||
|
||||
def vector_to_AP(vec):
|
||||
'''convert pybullet vector tuple to ArduPilot Vector3'''
|
||||
return Vector3(vec[0], -vec[1], -vec[2])
|
||||
|
||||
def quaternion_from_AP(q):
|
||||
'''convert ArduPilot quaternion to pybullet quaternion'''
|
||||
return [q.q[1], -q.q[2], -q.q[3], q.q[0]]
|
||||
|
||||
def to_tuple(vec3):
|
||||
'''convert a Vector3 to a tuple'''
|
||||
return (vec3.x, vec3.y, vec3.z)
|
||||
|
||||
def init():
|
||||
global time_now
|
||||
time_now = 0
|
||||
robot.position = [0,0,0]
|
||||
robot.orientation = [0,0,0,1]
|
||||
|
||||
def constrain(v,min_v,max_v):
|
||||
'''constrain a value'''
|
||||
if v < min_v:
|
||||
v = min_v
|
||||
if v > max_v:
|
||||
v = max_v
|
||||
return v
|
||||
|
||||
#robot.position = [ 0, 0, 2]
|
||||
#robot.orientation = quaternion_from_AP(Quaternion([math.radians(0), math.radians(0), math.radians(50)]))
|
||||
|
||||
def physics_step(pwm_in):
|
||||
|
||||
control_pwm(pwm_in)
|
||||
|
||||
world.step(sleep_dt=0)
|
||||
|
||||
global time_now
|
||||
time_now += TIME_STEP
|
||||
|
||||
# get the position orientation and velocity
|
||||
quaternion = quaternion_to_AP(robot.orientation)
|
||||
roll, pitch, yaw = quaternion.euler
|
||||
velocity = vector_to_AP(robot.linear_velocity)
|
||||
position = vector_to_AP(robot.position)
|
||||
|
||||
# get ArduPilot DCM matrix (rotation matrix)
|
||||
dcm = quaternion.dcm
|
||||
|
||||
# get gyro vector in body frame
|
||||
gyro = dcm.transposed() * vector_to_AP(robot.angular_velocity)
|
||||
|
||||
# calculate acceleration
|
||||
global last_velocity
|
||||
if last_velocity is None:
|
||||
last_velocity = velocity
|
||||
|
||||
accel = (velocity - last_velocity) * (1.0 / TIME_STEP)
|
||||
last_velocity = velocity
|
||||
|
||||
# add in gravity in earth frame
|
||||
accel.z -= GRAVITY_MSS
|
||||
|
||||
# convert accel to body frame
|
||||
accel = dcm.transposed() * accel
|
||||
|
||||
# convert to tuples
|
||||
accel = to_tuple(accel)
|
||||
gyro = to_tuple(gyro)
|
||||
position = to_tuple(position)
|
||||
velocity = to_tuple(velocity)
|
||||
euler = (roll, pitch, yaw)
|
||||
|
||||
return time_now,gyro,accel,position,euler,velocity
|
||||
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
sock.bind(('', 9002))
|
||||
sock.settimeout(0.1)
|
||||
|
||||
last_SITL_frame = -1
|
||||
connected = False
|
||||
frame_count = 0
|
||||
frame_time = time.time()
|
||||
print_frame_count = 1000
|
||||
|
||||
move_accel = 0.0
|
||||
last_move = time.time()
|
||||
|
||||
def move_view(keys):
|
||||
'''move camera target based on arrow keys'''
|
||||
global last_move, move_accel
|
||||
now = time.time()
|
||||
if now - last_move < 0.1:
|
||||
return
|
||||
last_move = now
|
||||
KEY_LEFT = 65295
|
||||
KEY_RIGHT = 65296
|
||||
KEY_UP = 65297
|
||||
KEY_DOWN = 65298
|
||||
global move_accel
|
||||
angle = None
|
||||
if KEY_LEFT in keys:
|
||||
angle = 90
|
||||
elif KEY_RIGHT in keys:
|
||||
angle = -90
|
||||
elif KEY_UP in keys:
|
||||
angle = 180
|
||||
elif KEY_DOWN in keys:
|
||||
angle = 0
|
||||
else:
|
||||
move_accel = 0
|
||||
return
|
||||
|
||||
caminfo = list(sim.sim.getDebugVisualizerCamera())
|
||||
target = caminfo[-1]
|
||||
dist = caminfo[-2]
|
||||
pitch = caminfo[-3]
|
||||
yaw = caminfo[-4]
|
||||
look = 90.0-yaw
|
||||
step = 0.3 + move_accel
|
||||
move_accel += 0.1
|
||||
move_accel = min(move_accel, 5)
|
||||
target = (target[0] + step*math.cos(radians(look+angle)),
|
||||
target[1] - step*math.sin(radians(look+angle)),
|
||||
target[2])
|
||||
sim.reset_debug_visualizer(dist, radians(yaw), radians(pitch), target)
|
||||
|
||||
while True:
|
||||
|
||||
py_time = time.time()
|
||||
|
||||
interface.step()
|
||||
move_view(interface.key_down)
|
||||
|
||||
try:
|
||||
data,address = sock.recvfrom(100)
|
||||
except Exception as ex:
|
||||
time.sleep(0.01)
|
||||
continue
|
||||
|
||||
parse_format = 'HHI16H'
|
||||
magic = 18458
|
||||
|
||||
if len(data) != struct.calcsize(parse_format):
|
||||
print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format)))
|
||||
continue
|
||||
|
||||
|
||||
decoded = struct.unpack(parse_format,data)
|
||||
|
||||
if magic != decoded[0]:
|
||||
print("Incorrect protocol magic %u should be %u" % (decoded[0], magic))
|
||||
continue
|
||||
|
||||
frame_rate_hz = decoded[1]
|
||||
frame_count = decoded[2]
|
||||
pwm = decoded[3:]
|
||||
|
||||
if frame_rate_hz != RATE_HZ:
|
||||
print("New frame rate %u" % frame_rate_hz)
|
||||
RATE_HZ = frame_rate_hz
|
||||
TIME_STEP = 1.0 / RATE_HZ
|
||||
sim.set_time_step(TIME_STEP)
|
||||
|
||||
# Check if the fame is in expected order
|
||||
if frame_count < last_SITL_frame:
|
||||
# Controller has reset, reset physics also
|
||||
init()
|
||||
print('Controller reset')
|
||||
elif frame_count == last_SITL_frame:
|
||||
# duplicate frame, skip
|
||||
print('Duplicate input frame')
|
||||
continue
|
||||
elif frame_count != last_SITL_frame + 1 and connected:
|
||||
print('Missed {0} input frames'.format(frame_count - last_SITL_frame - 1))
|
||||
last_SITL_frame = frame_count
|
||||
|
||||
if not connected:
|
||||
connected = True
|
||||
print('Connected to {0}'.format(str(address)))
|
||||
frame_count += 1
|
||||
|
||||
# physics time step
|
||||
phys_time,gyro,accel,pos,euler,velo = physics_step(pwm)
|
||||
|
||||
# build JSON format
|
||||
IMU_fmt = {
|
||||
"gyro" : gyro,
|
||||
"accel_body" : accel
|
||||
}
|
||||
JSON_fmt = {
|
||||
"timestamp" : phys_time,
|
||||
"imu" : IMU_fmt,
|
||||
"position" : pos,
|
||||
"attitude" : euler,
|
||||
"velocity" : velo
|
||||
}
|
||||
JSON_string = "\n" + json.dumps(JSON_fmt,separators=(',', ':')) + "\n"
|
||||
|
||||
# Send to AP
|
||||
sock.sendto(bytes(JSON_string,"ascii"), address)
|
||||
|
||||
# Track frame rate
|
||||
if frame_count % print_frame_count == 0:
|
||||
now = time.time()
|
||||
total_time = now - frame_time
|
||||
print("%.2f fps T=%.3f dt=%.3f" % (print_frame_count/total_time, phys_time, total_time))
|
||||
frame_time = now
|
||||
Loading…
Reference in New Issue