I have created a robot with the prototype plugin, and when testing the prototype by itself, it works great. I've created functions to drive it forward, turn left/right, and they work with no issues.
The problem is that when I distribute multiple prototypes over the arena, the same functionality starts behaving strangely. For example: I'll load in 3 robots and tell them all to drive forward. 2 of them will drive forward and the third one will just spin around and crash into the wall. There is no difference between the scripts or the prototype configuration, as I am using the "distribute" plugin on 1 prototype.
The next thing I noticed is that if multiple prototypes get near each other, they start behaving strangely. It seems like there is some kind of physics interaction that happens between the prototypes (almost like they are colliding). However, when I view the bounding boxes of the prototypes as they start behaving in this way, there show as not colliding.
Could this be a bug in the prototype plugin physics? Or have I done something completely wrong? Here is my code:
ARGoS Config:
Code: Select all
<?xml version = "1.0" ?>
<argos-configuration>
<framework>
<experiment length ="0"
ticks_per_second="30"
random_seed= "0"/>
</framework>
<controllers>
<lua_controller id="tellus_controller">
<actuators>
<joints implementation="default">
<joint id="fr_arm_wheel" mode="velocity" max_impulse="0.05f"/>
<joint id="fl_arm_wheel" mode="velocity" max_impulse="0.05f"/>
<joint id="bl_arm_wheel" mode="velocity" max_impulse="0.05f"/>
<joint id="br_arm_wheel" mode="velocity" max_impulse="0.05f"/>
<joint id="mr_arm_wheel" mode="velocity" max_impulse="0.05f"/>
<joint id="ml_arm_wheel" mode="velocity" max_impulse="0.05f"/>
<joint id="base_lidar" mode="velocity" max_impulse="0.05f"/>
</joints>
<radios implementation="default" medium="radios"/>
</actuators>
<sensors>
<joints implementation="default">
<joint id="base_lidar" mode="position"/>
<joint id="fr_arm_wheel" mode="velocity"/>
<joint id="fl_arm_wheel" mode="velocity"/>
<joint id="br_arm_wheel" mode="velocity"/>
<joint id="bl_arm_wheel" mode="velocity"/>
<joint id="mr_arm_wheel" mode="velocity"/>
<joint id="ml_arm_wheel" mode="velocity"/>
</joints>
<radios implementation="default" medium="radios"/>
<proximity implementation= "default" show_rays="true"/>
<positioning implementation = "default"/>
</sensors>
<params />
</lua_controller>
</controllers>
<arena size="4,4,5" center="0,0,0" positional_grid_size="1, 1, 1">
<floor id="floor"
source="image"
path="../textures/axis.png" />
<box id="wall_north" size="4,0.1,0.1" movable="false">
<body position="0,2,0" orientation="0,0,0" />
</box>
<box id="wall_south" size="4,0.1,0.1" movable="false">
<body position="0,-2,0" orientation="0,0,0" />
</box>
<box id="wall_east" size="0.1,4,0.1" movable="false">
<body position="2,0,0" orientation="0,0,0" />
</box>
<box id="wall_west" size="0.1,4,0.1" movable="false">
<body position="-2,0,0" orientation="0,0,0" />
</box>
<distribute>
<position method="uniform" min="-1.5,-1.5,0" max="-1,-1,0" />
<orientation method="gaussian" mean="0,0,0" std_dev="0,0,0" />
<entity quantity="3" max_trials="100">
<prototype id="tellus" movable="true">
<body position = "0,0,0.1" orientation = "0,0,0"/>
<controller config="tellus_controller"/>
<links ref = "base">
<link id="base" geometry="box" size="0.1,0.05,0.02" mass="1" position="0,0,0" orientation="0,0,0"/>
<link id="lidar_servo" geometry="cylinder" radius="0.01" height="0.02" mass="0.2" position="0.04,0,0.02" orientation="0,0,0"/>
<link id="fl_arm" geometry="box" size="0.01,0.05,0.01" mass="0.1" position="0.05,0.03,-0.01" orientation="90,45,0"/>
<link id="fr_arm" geometry="box" size="0.01,0.05,0.01" mass="0.1" position="0.05,-0.03,-0.01" orientation="90,45,0"/>
<link id="ml_arm" geometry="box" size="0.01,0.05,0.01" mass="0.1" position="-0.025,0.03,-0.01" orientation="90,45,0"/>
<link id="mr_arm" geometry="box" size="0.01,0.05,0.01" mass="0.1" position="-0.025,-0.03,-0.01" orientation="90,45,0"/>
<link id="bl_arm" geometry="box" size="0.01,0.05,0.01" mass="0.1" position="-0.05,0.03,-0.01" orientation="90,-45,0"/>
<link id="br_arm" geometry="box" size="0.01,0.05,0.01" mass="0.1" position="-0.05,-0.03,-0.01" orientation="90,-45,0"/>
<link id= "fl_wheel" geometry ="cylinder" radius="0.02" height= "0.01" mass="0.1" position="0.06,0.045,-0.02" orientation="0,0,90"/>
<link id= "fr_wheel" geometry ="cylinder" radius="0.02" height= "0.01" mass="0.1" position="0.06,-0.04,-0.02" orientation="0,0,90"/>
<link id= "ml_wheel" geometry ="cylinder" radius="0.02" height= "0.01" mass="0.1" position="0,0.045,-0.02" orientation="0,0,90"/>
<link id= "mr_wheel" geometry ="cylinder" radius="0.02" height= "0.01" mass="0.1" position="0,-0.045,-0.02" orientation="0,0,90"/>
<link id= "bl_wheel" geometry ="cylinder" radius="0.02" height= "0.01" mass="0.1" position="-0.06,0.045,-0.02" orientation="0,0,90"/>
<link id= "br_wheel" geometry ="cylinder" radius="0.02" height= "0.01" mass="0.1" position="-0.06,-0.04,-0.02" orientation="0,0,90"/>
</links>
<joints>
<joint id="base_lidar" type="revolute" axis="0,0,1">
<parent link="base" position="0.04,0,0.02" orientation="0,0,0"/>
<child link="lidar_servo" position="0,0,0.005" orientation="0,0,0"/>
</joint>
<joint id="base_flarm" type="fixed">
<parent link="base" position="0.05,0.03,-0.01" orientation="90,45,0"/>
<child link="fl_arm" position="0,0,0" orientation="0,0,0"/>
</joint>
<joint id="base_frarm" type="fixed">
<parent link="base" position="0.05,-0.03,-0.01" orientation="90,45,0"/>
<child link="fr_arm" position="0,0,0" orientation="0,0,0"/>
</joint>
<joint id="base_mlarm" type="fixed">
<parent link="base" position="-0.025,0.03,-0.01" orientation="90,45,0"/>
<child link="ml_arm" position="0,0,0" orientation="0,0,0"/>
</joint>
<joint id="base_mrarm" type="fixed">
<parent link="base" position="-0.025,-0.03,-0.01" orientation="90,45,0"/>
<child link="mr_arm" position="0,0,0" orientation="0,0,0"/>
</joint>
<joint id="base_blarm" type="fixed">
<parent link="base" position="-0.05,0.03,-0.01" orientation="90,-45,0"/>
<child link="bl_arm" position="0,0,0" orientation="0,0,0"/>
</joint>
<joint id="base_brarm" type="fixed">
<parent link="base" position="-0.05,-0.03,-0.01" orientation="90,-45,0"/>
<child link="br_arm" position="0,0,0" orientation="0,0,0"/>
</joint>
<joint id="fl_arm_wheel" type="revolute" axis = "0,0,1">
<parent link="fl_arm" position="0.015,-0.025,0" orientation="0,0,0"/>
<child link="fl_wheel" position="0,0,0" orientation="0,90,0"/>
</joint>
<joint id="fr_arm_wheel" type="revolute" axis = "0,0,1">
<parent link="fr_arm" position="-0.005,-0.025,0" orientation="0,0,0"/>
<child link="fr_wheel" position="0,0,0" orientation="0,90,0"/>
</joint>
<joint id="ml_arm_wheel" type="revolute" axis = "0,0,1">
<parent link="ml_arm" position="0.015,-0.025,0" orientation="0,0,0"/>
<child link="ml_wheel" position="0,0,0" orientation="0,90,0"/>
</joint>
<joint id="mr_arm_wheel" type="revolute" axis = "0,0,1">
<parent link="mr_arm" position="-0.005,-0.025,0" orientation="0,0,0"/>
<child link="mr_wheel" position="0,0,0" orientation="0,90,0"/>
</joint>
<joint id="bl_arm_wheel" type="revolute" axis = "0,0,1">
<parent link="bl_arm" position="0.015,0.025,0" orientation="0,0,0"/>
<child link="bl_wheel" position="0,0,0" orientation="0,90,0"/>
</joint>
<joint id="br_arm_wheel" type="revolute" axis = "0,0,1">
<parent link="br_arm" position="-0.005,0.025,0" orientation="0,0,0"/>
<child link="br_wheel" position="0,0,0" orientation="0,90,0"/>
</joint>
</joints>
<devices>
<radios medium="radios">
<radio anchor="base" duplex_mode="half" range="1"
position="0,0,0.0075"/>
</radios>
<proximity_sensors>
<sensor anchor="lidar_servo" position="0,0,0" offset="0,0,0.02" direction="1,0,0" range="1" show_rays="true"/>
</proximity_sensors>
</devices>
</prototype>
</entity>
</distribute>
</arena>
<physics_engines>
<dynamics3d id="dyn3d" default_friction="2.0">
<floor height="0.01" friction="0.1"/>
<gravity g="10" />
</dynamics3d>
</physics_engines>
<media>
<radio id="radios"/>
</media>
<visualization>
<qt-opengl lua_editor="true">
<camera>
<placements>
<placement index="0" position="0,0,8.14689" look_at="0,0,0" up="1,0,0" lens_focal_length="5" />
</placements>
</camera>
</qt-opengl>
</visualization>
</argos-configuration>
Code: Select all
function driveForward(velocity)
robot.joints.bl_arm_wheel.set_target(velocity)
robot.joints.ml_arm_wheel.set_target(0-velocity)
robot.joints.fl_arm_wheel.set_target(0-velocity)
robot.joints.br_arm_wheel.set_target(0-velocity)
robot.joints.mr_arm_wheel.set_target(velocity)
robot.joints.fr_arm_wheel.set_target(0-velocity)
end
function init()
-- put your code here
end
--[[ This function is executed at each time step
It must contain the logic of your controller ]]
function step()
-- put your code here
driveForward(10)
end
--[[ This function is executed every time you press the 'reset'
button in the GUI. It is supposed to restore the state
of the controller to whatever it was right after init() was
called. The state of sensors and actuators is reset
automatically by ARGoS. ]]
function reset()
-- put your code here
end
--[[ This function is executed only once, when the robot is removed
from the simulation ]]
function destroy()
-- put your code here
end
