Multi-robot collaboration for pushing a box towards a goal #121
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
* Add ca_swarm package. Include basic swarm formation controller node. * Get swarm_controller_node hooked up in launch. * Add missing numpy module alias. * Clean up ca_swarm CMakeLists.txt * Use base geometry_msgs.msg.Twist message, w/o stamps. * Clean up ca_swarm swarm_controller_node. * Clean up cooperative two robots sim demo. * Fix Michel's approach
eborghi10
commented
Nov 13, 2019
| <robot xmlns:xacro="http://ros.org/wiki/xacro"> | ||
|
|
||
| <xacro:macro name="dummy_inertia"> | ||
| <inertial> |
There was a problem hiding this comment.
Call inertial_cuboid instead of creating the inertial from scratch.
|
|
||
| <!-- Roomblock --> | ||
| <xacro:roomblock parent="base_link" visualize="$(arg visualize)"/> | ||
| <!-- <xacro:roomblock parent="base_link" visualize="$(arg visualize)"/> --> |
There was a problem hiding this comment.
Check if roomblock affects how the robot moves the block. If so, check how we can mitigate this effect.
| <xacro:caster_wheel name="caster_front"> | ||
| <origin xyz="0.13 0 0" rpy="0 ${pi/2} ${pi/2}" /> | ||
| </xacro:caster_wheel> | ||
| <xacro:caster_wheel name="caster_back"> |
There was a problem hiding this comment.
Is this new caster necessary? Check mass distributions.
| <robot xmlns:xacro="http://ros.org/wiki/xacro"> | ||
|
|
||
| <xacro:macro name="caster_wheel"> | ||
| <xacro:macro name="caster_wheel" params="name *origin"> |
There was a problem hiding this comment.
Put these changes into a new PR.
| <link name="${name_side_left}_cliff_sensor_link"> | ||
| <xacro:dummy_inertia/> | ||
| </link> | ||
| <link name="${name_side_left}_cliff_sensor_link"/> |
There was a problem hiding this comment.
Move this into a new PR.
|
|
||
| R = v / w if w != 0. else np.inf | ||
|
|
||
| self._box_error_v = v - self._box_v |
There was a problem hiding this comment.
Velocity control of the box based on information given by move_base.
| w_box = 0. | ||
|
|
||
| if np.allclose(robot.b_T_i, np.identity(4, dtype=np.float64)): | ||
| robot.b_T_i = transformations.compose_matrix(angles=[0., 0., yaw], translate=[x, y, 0.]) |
There was a problem hiding this comment.
# First time, get the initial pose between the robot and the box (bTi)
| box_mass = 1 # kg | ||
|
|
||
| dv = self._box_error_v if R == np.inf else self._box_error_w * (R - y) | ||
| i_T_tau = transformations.compose_matrix(translate=[max(0, dv * box_mass / K), 0., 0]) |
There was a problem hiding this comment.
# Calculate how much the robot needs to "penetrate" the box in order to reach the desired box velocities.
| dv = self._box_error_v if R == np.inf else self._box_error_w * (R - y) | ||
| i_T_tau = transformations.compose_matrix(translate=[max(0, dv * box_mass / K), 0., 0]) | ||
|
|
||
| c_T_tau = transformations.concatenate_matrices( |
There was a problem hiding this comment.
# Get the desired (x, y, theta) pose for the robots that move the box to the desired pose.
| rho = np.linalg.norm([xd, yd]) | ||
| theta = np.arctan2(yd, xd) | ||
|
|
||
| K_theta = 1.0 |
There was a problem hiding this comment.
# Parameters of the algorithm
eborghi10
force-pushed
the
kinetic-devel
branch
4 times, most recently
from
23e75d6 to
05fa7b9
Compare
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
TODO (@eborghi10 ):
This PR is a collaboration between @hidmic @dmnunez1993 @jlurgo and me.