Skip to content

Commit 9d3dbb8

Browse files
YadunundYadunund
authored
Bimanual demo within Integrated World (#240)
* Spawn second rexrov Signed-off-by: Yadunund <[email protected]> * Add moveit manager Signed-off-by: Yadunund <[email protected]> * Update collision mesh for electrical panel Signed-off-by: Yadunund <[email protected]> * WIP Signed-off-by: Yadunund <[email protected]> * Updated script, friction, collision Signed-off-by: Yadunund <[email protected]> * Able to grasp Signed-off-by: Yadunund <[email protected]> * More positions Signed-off-by: Yadunund <[email protected]> * Cleanup Signed-off-by: Yadunund <[email protected]> * Revert mating tolerances Signed-off-by: Yadunund <[email protected]> * Install bimanual script Signed-off-by: Yadunund <[email protected]> * Change exec_depend to run_depend Signed-off-by: Yadunund <[email protected]> * Updated launch file Signed-off-by: Yadunund <[email protected]> * Update join positions and tune plug physics for better gripping Signed-off-by: Yadunund <[email protected]> * Cleanup Signed-off-by: Yadunund <[email protected]> * Undo dependency deletion Signed-off-by: Yadunund <[email protected]>
1 parent d7d8091 commit 9d3dbb8

File tree

10 files changed

+250
-25
lines changed

10 files changed

+250
-25
lines changed

‎examples/dave_demo_launch/launch/dave_integrated_demo.launch‎

Lines changed: 44 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,18 @@
11
<?xml version="1.0"?>
22
<launch>
33
<arg name="gui" default="true"/>
4-
<arg name="paused" default="false"/>
4+
<arg name="paused" default="true"/>
55
<arg name="world_name" default="$(find dave_worlds)/worlds/dave_integrated.world"/>
6-
<arg name="namespace" default="rexrov"/>
6+
<arg name="namespace" default="rexrov0"/>
77
<arg name="set_timeout" default="false"/>
88
<arg name="timeout" default="0.0"/>
99
<arg name="velocity_control" default="true"/>
1010
<arg name="joy_id" default="0"/>
11+
<arg name="debug" default="false"/>
12+
<arg name="controller_type" default="effort"/>
13+
14+
<arg unless="$(arg debug)" name="launch-prefix" value=" "/>
15+
<arg if="$(arg debug)" name="launch-prefix" value="gdb -ex run --args"/>
1116

1217
<!-- use Gazebo's empty_world.launch with dave_ocean_waves.world -->
1318
<include file="$(find gazebo_ros)/launch/empty_world.launch">
@@ -16,7 +21,7 @@
1621
<arg name="use_sim_time" value="true"/>
1722
<arg name="gui" value="$(arg gui)"/>
1823
<arg name="headless" value="false"/>
19-
<arg name="debug" value="false"/>
24+
<arg name="debug" value="$(arg debug)"/>
2025
<arg name="verbose" value="false"/>
2126
</include>
2227

@@ -33,7 +38,7 @@
3338

3439
<!-- rexrov robot with oberon7 arm -->
3540
<include file="$(find rexrov_description)/launch/upload_rexrov_oberon7.launch">
36-
<arg name="namespace" value="rexrov"/>
41+
<arg name="namespace" value="rexrov0"/>
3742
<arg name="x" value="-21.0"/>
3843
<arg name="y" value="-34.0"/>
3944
<arg name="z" value="-108.0"/>
@@ -49,18 +54,51 @@
4954

5055
<!-- joystick control for rexrov and oberon7, no velocity control-->
5156
<include unless="$(arg velocity_control)" file="$(find uuv_control_cascaded_pid)/launch/joy_accel.launch">
57+
<arg name="uuv_name" value="rexrov0" />
5258
<arg name="model_name" value="rexrov"/>
5359
<arg name="joy_id" value="$(arg joy_id)"/>
5460
</include>
5561

5662
<!-- joint control for oberon7 -->
5763
<include file="$(find oberon7_control)/launch/joint_control.launch">
58-
<arg name="uuv_name" value="$(arg namespace)"/>
64+
<arg name="uuv_name" value="rexrov0"/>
65+
</include>
66+
67+
<!-- Include a second rexrov with dual arms -->
68+
<include file="$(find rexrov_description)/launch/upload_rexrov_oberon7_moveit.launch">
69+
<arg name="namespace" value="rexrov"/>
70+
<arg name="controller_type" value="$(arg controller_type)"/>
71+
<arg name="x" value="-187.77"/>
72+
<arg name="y" value="115.20"/>
73+
<arg name="z" value="-155.35"/>
74+
<arg name="roll" value="-0.01"/>
75+
<arg name="pitch" value="0.177"/>
76+
<arg name="yaw" value="3.14"/>
77+
</include>
78+
79+
<!-- Load general controllers -->
80+
<include file="$(find oberon7_gazebo)/launch/controller_utils.launch"/>
81+
82+
<rosparam file="$(find oberon7_gazebo)/controller/oberon7_controllers.yaml" command="load"/>
83+
84+
<!-- Load chosen controller types -->
85+
<group if="$(eval controller_type == 'effort')">
86+
<node name="controllers_spawner" pkg="controller_manager" type="spawner" args="hand_effort_r hand_effort_l arm_effort_l arm_effort_r" respawn="false" output="screen"/>
87+
</group>
88+
89+
<group if="$(eval controller_type == 'position')">
90+
<node name="controllers_spawner" pkg="controller_manager" type="spawner" args="hand_position_r hand_position_l arm_position_l arm_position_r" respawn="false" output="screen"/>
91+
</group>
92+
93+
<include file="$(find uuv_control_cascaded_pid)/launch/joy_velocity.launch">
94+
<arg name="uuv_name" value="rexrov" />
95+
<arg name="model_name" value="rexrov" />
96+
<arg name="joy_id" value="1"/>
5997
</include>
6098

6199
<!-- Spawn socket platform for electrical lead station -->
62100
<include file="$(find plug_and_socket_description)/launch/upload_socket_platform.launch"/>
63101
<node name="spawn_socket" pkg="gazebo_ros" type="spawn_model"
64-
args="-urdf -param socket_platform -model socket_platform -x -190.5 -y 115.15 -z -158.5 -Y -1.57079632679" respawn="false" output="screen" />
102+
args="-urdf -param socket_platform -model socket_platform -x -190.5 -y 115.15 -z -158.459771 -Y -1.57079632679 -unpause" respawn="false" output="screen" />
65103

66104
</launch>

‎examples/dave_nodes/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -51,4 +51,5 @@ catkin_install_python(PROGRAMS
5151
src/dvl_state_and_gradient_uuvsim.py
5252
src/bimanual_simple_demo.py
5353
src/teleport_vehicle.py
54+
src/bimanual_integrated_world.py
5455
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

‎examples/dave_nodes/src/bimanual_integrated_world.py‎

Lines changed: 168 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,168 @@
1+
#!/usr/bin/env python
2+
3+
from __future__ import print_function
4+
from six.moves import input
5+
6+
import sys
7+
import copy
8+
import rospy
9+
import moveit_commander
10+
import moveit_msgs.msg
11+
import geometry_msgs.msg
12+
import tf
13+
import numpy as np
14+
15+
from gazebo_msgs.msg import ModelStates, ModelState
16+
17+
try:
18+
from math import pi, tau, dist, fabs, cos
19+
except:
20+
from math import pi, fabs, cos, sqrt
21+
22+
tau = 2.0 * pi
23+
24+
def dist(p, q):
25+
return sqrt(sum((p_i - q_i) ** 2.0 for p_i, q_i in zip(p, q)))
26+
27+
28+
from std_msgs.msg import String
29+
from moveit_commander.conversions import pose_to_list
30+
31+
32+
class MoveGroupPythonInterface(object):
33+
def __init__(self):
34+
super(MoveGroupPythonInterface, self).__init__()
35+
36+
# Initialize `moveit_commander` and node:
37+
moveit_commander.roscpp_initialize(sys.argv)
38+
rospy.init_node("move_group_python_interface", anonymous=True)
39+
40+
# Instantiate a `RobotCommander` object. Provides robot's
41+
# kinematic model and current joint states
42+
self.robot = moveit_commander.RobotCommander()
43+
44+
# Instantiate a `PlanningSceneInterface` object. Provides a remote interface
45+
# for getting, setting, and updating robot's state
46+
self.scene = moveit_commander.PlanningSceneInterface()
47+
48+
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
49+
## to a planning group (group of joints).
50+
self.move_group_hand_r = moveit_commander.MoveGroupCommander("hand_r")
51+
self.move_group_hand_l = moveit_commander.MoveGroupCommander("hand_l")
52+
self.move_group_arm_r = moveit_commander.MoveGroupCommander("arm_r")
53+
self.move_group_arm_l = moveit_commander.MoveGroupCommander("arm_l")
54+
55+
## Set planner
56+
self.move_group_arm_r.set_planner_id("RRTConnect")
57+
self.move_group_arm_l.set_planner_id("RRTConnect")
58+
59+
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
60+
## trajectories in Rviz:
61+
self.display_trajectory_publisher = rospy.Publisher(
62+
"/move_group/display_planned_path",
63+
moveit_msgs.msg.DisplayTrajectory,
64+
queue_size=20,
65+
)
66+
67+
# Print the entire state of the robot:
68+
print("============ Printing robot state")
69+
print(self.robot.get_current_state())
70+
print("")
71+
72+
# Misc variables
73+
self.box_name = ""
74+
75+
# Logging starting pose. There are better ways to do this.
76+
self.arm_l_start = self.move_group_arm_l.get_current_pose().pose
77+
self.arm_r_start = self.move_group_arm_r.get_current_pose().pose
78+
79+
self.rate = rospy.Rate(1.0)
80+
81+
def move_gripper(self, gripper='right', command='open'):
82+
print()
83+
print(f"=========={gripper} gripper will {command}...==========")
84+
85+
move_group = self.move_group_hand_r
86+
if gripper == "left":
87+
move_group = self.move_group_hand_l
88+
89+
joint_goal = move_group.get_current_joint_values()
90+
print(f"gripper current joints:{joint_goal}")
91+
joint_goal[0] = 0.5
92+
joint_goal[1] = 0.0
93+
joint_goal[2] = 0.5
94+
joint_goal[3] = 0.0
95+
if command == 'close':
96+
close = 0.17453292519943295
97+
# 0.1144640137963143
98+
joint_goal[0] = close/10.0
99+
joint_goal[2] = close
100+
101+
move_group.go(joint_goal, wait=True)
102+
move_group.stop()
103+
print(self.robot.get_current_state())
104+
105+
def move_arm(self, arm='right', joint_angles=[]):
106+
print()
107+
print(f"==========Moving {arm} to goal:{joint_angles}...==========")
108+
109+
if len(joint_angles) < 6:
110+
print("Error: Fewer than 6 joint angles provided. Aborting...")
111+
return
112+
113+
move_group = self.move_group_arm_r
114+
if arm == 'left':
115+
move_group = self.move_group_arm_l
116+
# Call the planner to compute the plan and execute it.
117+
plan = move_group.go(joint_angles, wait=True)
118+
# Call `stop()` to ensure there is no residual movement
119+
move_group.stop()
120+
# Clear your targets after planning with poses.
121+
move_group.clear_pose_targets()
122+
123+
print(self.robot.get_current_state())
124+
125+
126+
def cartesian_move_z(self, move_group, z, scale=1):
127+
waypoints = []
128+
wpose = move_group.get_current_pose().pose
129+
wpose.position.z += z
130+
waypoints.append(copy.deepcopy(wpose))
131+
(plan, fraction) = move_group.compute_cartesian_path(
132+
waypoints, 0.01, 0.0)
133+
move_group.execute(plan, wait=True)
134+
# return plan, fraction
135+
136+
def main():
137+
try:
138+
print("Setting up moveit commander")
139+
bimanual_demo = MoveGroupPythonInterface()
140+
bimanual_demo.move_gripper('left', 'close')
141+
bimanual_demo.move_arm(
142+
'left',
143+
[0.2725458466968526, 0.3199954120186353, 0.27050573169303094, -0.05612724177451451, -0.3366234745727277, 0.27816477039994353])
144+
bimanual_demo.move_gripper('left', 'open')
145+
bimanual_demo.move_arm(
146+
'left',
147+
[0.2525493202907798, -0.20498309678016832, 0.8860128543728201, -0.026684396779588802, -0.4685281195659303, 0.25894079241234275])
148+
bimanual_demo.move_gripper('left', 'close')
149+
bimanual_demo.move_arm(
150+
'left',
151+
[0.22463184903468725,0.5640109055722806,-0.010189445663893758,-0.0029447975262430984,-0.40265756418968657,0.2046361271467092])
152+
bimanual_demo.move_arm(
153+
'left',
154+
[-0.4396898837134099,0.5321104980211642,0.1817969793457925,0.2751617627062951,-0.5456892082990646,-0.7223044226272037])
155+
bimanual_demo.move_arm(
156+
'left',
157+
[-0.5076864582614361, 0.6430632572402184, -0.1964135304533572, 0.4261104261543335, -0.31872944826848815, -1.014079334617769])
158+
bimanual_demo.move_arm(
159+
'left',
160+
[-0.5605443400556704, 0.6194194950284646, -0.43562651809666936, -0.6993978537986612, 0.6179626637170913, 0.0903226598271187])
161+
162+
except rospy.ROSInterruptException:
163+
return
164+
except KeyboardInterrupt:
165+
return
166+
167+
if __name__ == "__main__":
168+
main()

‎models/dave_object_models/models/electrical_panel/meshes/COLLISION-Underwater-Frame.dae‎

Lines changed: 11 additions & 12 deletions
Large diffs are not rendered by default.

‎models/dave_object_models/models/electrical_panel/model.sdf‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,7 @@
8686
<sensorPlateLink>m_to_f_sensor_plate</sensorPlateLink>
8787
<plugModel>m_to_f_plug</plugModel>
8888
<plugLink>m_to_f_plug</plugLink>
89+
<matingForce>0.0</matingForce>
8990
<rollAlignmentTolerance>0.3</rollAlignmentTolerance>
9091
<pitchAlignmentTolerance>0.3</pitchAlignmentTolerance>
9192
<yawAlignmentTolerance>0.3</yawAlignmentTolerance>

‎models/dave_object_models/models/m_to_f_plug/model.sdf‎

Lines changed: 16 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,21 @@
2424
<scale>0.1 0.1 0.1</scale>
2525
</mesh>
2626
</geometry>
27+
<surface>
28+
<contact>
29+
<ode>
30+
<kp>1e6</kp>
31+
<kd>1.0</kd>
32+
<min_depth>0.001</min_depth>
33+
</ode>
34+
</contact>
35+
<friction>
36+
<ode>
37+
<mu1>100.0</mu1>
38+
<mu2>50.0</mu2>
39+
</ode>
40+
</friction>
41+
</surface>
2742
</collision>
2843

2944
<!-- <collision name ='stand'>
@@ -43,7 +58,7 @@
4358
<iyz>0</iyz>
4459
<izz>0.04</izz>
4560
</inertia>
46-
<mass>0.5</mass>
61+
<mass>0.2</mass>
4762
</inertial>
4863
</link>
4964

‎models/dave_worlds/worlds/dave_integrated.world‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -163,7 +163,7 @@
163163
<!-- Electrical flying lead -->
164164
<include>
165165
<uri>model://m_to_f_plug</uri>
166-
<pose>-191.06 117.25 -155.84 0 -1.570777 0</pose>
166+
<pose>-189.80 114.41 -157.611077 0 -1.570777 0</pose>
167167
</include>
168168

169169
<include>
@@ -173,7 +173,7 @@
173173

174174
<model name="platform">
175175
<static>true</static>
176-
<pose>-191.06 117.5 -156 0 0 0</pose>
176+
<pose>-191.06 117.5 -155.8 0 0 0</pose>
177177
<link name="link">
178178
<collision name="collision">
179179
<geometry>

‎urdf/robots/rexrov_description/launch/upload_rexrov_oberon7_moveit.launch‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -6,9 +6,11 @@
66
<arg name="x" default="4"/>
77
<arg name="y" default="2"/>
88
<arg name="z" default="-91"/>
9+
<arg name="roll" default="0.0"/>
10+
<arg name="pitch" default="0.0"/>
911
<arg name="yaw" default="3.141592"/>
1012

1113
<param name="robot_description" command="$(find xacro)/xacro '$(find rexrov_description)/urdf/rexrov_robot.urdf.xacro' inertial_reference_frame:=world controller_type:=$(arg controller_type)"/>
1214

13-
<node name="urdf_spawner" pkg="uuv_descriptions" type="spawn_model" respawn="false" output="screen" args="-urdf -x $(arg x) -y $(arg y) -z $(arg z) -Y $(arg yaw) -model $(arg namespace) -param robot_description"/>
14-
</launch>
15+
<node name="urdf_spawner" pkg="uuv_descriptions" type="spawn_model" respawn="false" output="screen" args="-urdf -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw) -model $(arg namespace) -param robot_description"/>
16+
</launch>

‎urdf/robots/rexrov_oberon7_moveit/config/rexrov.srdf‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -192,6 +192,7 @@
192192
<disable_collisions link1="oberon7_l/finger_right" link2="rexrov/thruster_5" reason="Never"/>
193193
<disable_collisions link1="oberon7_l/finger_right" link2="rexrov/thruster_6" reason="Never"/>
194194
<disable_collisions link1="oberon7_l/finger_right" link2="rexrov/thruster_7" reason="Never"/>
195+
<disable_collisions link1="oberon7_l/finger_right" link2="oberon7_l/wrist_link" reason="Never"/>
195196
<disable_collisions link1="oberon7_l/finger_tip_left" link2="oberon7_l/finger_tip_right" reason="Never"/>
196197
<disable_collisions link1="oberon7_l/finger_tip_left" link2="oberon7_l/forearm" reason="Never"/>
197198
<disable_collisions link1="oberon7_l/finger_tip_left" link2="oberon7_l/wrist_link" reason="Never"/>

‎urdf/robots/rexrov_oberon7_moveit/package.xml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -24,8 +24,8 @@
2424
<run_depend>moveit_ros_visualization</run_depend>
2525
<run_depend>moveit_setup_assistant</run_depend>
2626
<run_depend>moveit_simple_controller_manager</run_depend>
27-
<exec_depend>moveit_commander</exec_depend>
28-
<exec_depend>moveit_planners</exec_depend>
27+
<run_depend>moveit_commander</run_depend>
28+
<run_depend>moveit_planners</run_depend>
2929
<run_depend>joint_state_publisher</run_depend>
3030
<run_depend>joint_state_publisher_gui</run_depend>
3131
<run_depend>robot_state_publisher</run_depend>

0 commit comments

Comments
 (0)