Bimanual demo within Integrated World

examples/dave_demo_launch/launch/dave_integrated_demo.launch

@@ -1,13 +1,18 @@
 <?xml version="1.0"?>
 <launch>
     <arg name="gui" default="true"/>
-    <arg name="paused" default="false"/>
+    <arg name="paused" default="true"/>
     <arg name="world_name" default="$(find dave_worlds)/worlds/dave_integrated.world"/>
-    <arg name="namespace" default="rexrov"/>
+    <arg name="namespace" default="rexrov0"/>
     <arg name="set_timeout" default="false"/>
     <arg name="timeout" default="0.0"/>
     <arg name="velocity_control" default="true"/>
     <arg name="joy_id" default="0"/>
+    <arg name="debug" default="false"/>
+	  <arg name="controller_type" default="effort"/>
+
+    <arg unless="$(arg debug)" name="launch-prefix" value=" "/>
+    <arg if="$(arg debug)" name="launch-prefix" value="gdb -ex run --args"/>
 
     <!-- use Gazebo's empty_world.launch with dave_ocean_waves.world -->
     <include file="$(find gazebo_ros)/launch/empty_world.launch">
@@ -16,7 +21,7 @@
         <arg name="use_sim_time" value="true"/>
         <arg name="gui" value="$(arg gui)"/>
         <arg name="headless" value="false"/>
-        <arg name="debug" value="false"/>
+        <arg name="debug" value="$(arg debug)"/>
         <arg name="verbose" value="false"/>
     </include>
 
@@ -33,7 +38,7 @@
 
     <!-- rexrov robot with oberon7 arm -->
     <include file="$(find rexrov_description)/launch/upload_rexrov_oberon7.launch">
-        <arg name="namespace" value="rexrov"/>
+        <arg name="namespace" value="rexrov0"/>
         <arg name="x" value="-21.0"/>
         <arg name="y" value="-34.0"/>
         <arg name="z" value="-108.0"/>
@@ -49,18 +54,51 @@
 
     <!-- joystick control for rexrov and oberon7, no velocity control-->
     <include unless="$(arg velocity_control)" file="$(find uuv_control_cascaded_pid)/launch/joy_accel.launch">
+      	<arg name="uuv_name" value="rexrov0" />
         <arg name="model_name" value="rexrov"/>
         <arg name="joy_id" value="$(arg joy_id)"/>
     </include>
 
     <!-- joint control for oberon7 -->
     <include file="$(find oberon7_control)/launch/joint_control.launch">
-        <arg name="uuv_name" value="$(arg namespace)"/>
+        <arg name="uuv_name" value="rexrov0"/>
+    </include>
+
+    <!-- Include a second rexrov with dual arms -->
+  	<include file="$(find rexrov_description)/launch/upload_rexrov_oberon7_moveit.launch">
+      <arg name="namespace" value="rexrov"/>
+		  <arg name="controller_type" value="$(arg controller_type)"/>
+      <arg name="x" value="-187.77"/>
+      <arg name="y" value="115.20"/>
+      <arg name="z" value="-155.35"/>
+      <arg name="roll" value="-0.01"/>
+      <arg name="pitch" value="0.177"/>
+      <arg name="yaw" value="3.14"/>
+    </include>
+
+  	<!-- Load general controllers -->
+  	<include file="$(find oberon7_gazebo)/launch/controller_utils.launch"/>
+
+  	<rosparam file="$(find oberon7_gazebo)/controller/oberon7_controllers.yaml" command="load"/>
+
+  	<!-- Load chosen controller types -->
+  	<group if="$(eval controller_type == 'effort')">
+  		<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"/>
+  	</group>
+
+  	<group if="$(eval controller_type == 'position')">
+  		<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"/>
+  	</group>
+
+    <include file="$(find uuv_control_cascaded_pid)/launch/joy_velocity.launch">
+    	<arg name="uuv_name" value="rexrov" />
+    	<arg name="model_name" value="rexrov" />
+    	<arg name="joy_id" value="1"/>
     </include>
 
     <!-- Spawn socket platform for electrical lead station -->
     <include file="$(find plug_and_socket_description)/launch/upload_socket_platform.launch"/>
     <node name="spawn_socket" pkg="gazebo_ros" type="spawn_model"
-            args="-urdf -param socket_platform -model socket_platform -x -190.5 -y 115.15 -z -158.5 -Y -1.57079632679" respawn="false" output="screen" />
+            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" />
 
 </launch>

examples/dave_nodes/CMakeLists.txt

@@ -51,4 +51,5 @@ catkin_install_python(PROGRAMS
   src/dvl_state_and_gradient_uuvsim.py
   src/bimanual_simple_demo.py
   src/teleport_vehicle.py
+  src/bimanual_integrated_world.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

examples/dave_nodes/src/bimanual_integrated_world.py

@@ -0,0 +1,168 @@
+#!/usr/bin/env python
+
+from __future__ import print_function
+from six.moves import input
+
+import sys
+import copy
+import rospy
+import moveit_commander
+import moveit_msgs.msg
+import geometry_msgs.msg
+import tf
+import numpy as np
+
+from gazebo_msgs.msg import ModelStates, ModelState
+
+try:
+    from math import pi, tau, dist, fabs, cos
+except:
+    from math import pi, fabs, cos, sqrt
+
+    tau = 2.0 * pi
+
+    def dist(p, q):
+        return sqrt(sum((p_i - q_i) ** 2.0 for p_i, q_i in zip(p, q)))
+
+
+from std_msgs.msg import String
+from moveit_commander.conversions import pose_to_list
+
+
+class MoveGroupPythonInterface(object):
+    def __init__(self):
+        super(MoveGroupPythonInterface, self).__init__()
+
+        # Initialize `moveit_commander` and node:
+        moveit_commander.roscpp_initialize(sys.argv)
+        rospy.init_node("move_group_python_interface", anonymous=True)
+
+        # Instantiate a `RobotCommander` object. Provides robot's
+        # kinematic model and current joint states
+        self.robot = moveit_commander.RobotCommander()
+
+        # Instantiate a `PlanningSceneInterface` object. Provides a remote interface
+        # for getting, setting, and updating robot's state
+        self.scene = moveit_commander.PlanningSceneInterface()
+
+        ## Instantiate a `MoveGroupCommander`_ object.  This object is an interface
+        ## to a planning group (group of joints).
+        self.move_group_hand_r = moveit_commander.MoveGroupCommander("hand_r")
+        self.move_group_hand_l = moveit_commander.MoveGroupCommander("hand_l")
+        self.move_group_arm_r = moveit_commander.MoveGroupCommander("arm_r")
+        self.move_group_arm_l = moveit_commander.MoveGroupCommander("arm_l")
+
+        ## Set planner
+        self.move_group_arm_r.set_planner_id("RRTConnect")
+        self.move_group_arm_l.set_planner_id("RRTConnect")
+
+        ## Create a `DisplayTrajectory`_ ROS publisher which is used to display
+        ## trajectories in Rviz:
+        self.display_trajectory_publisher = rospy.Publisher(
+            "/move_group/display_planned_path",
+            moveit_msgs.msg.DisplayTrajectory,
+            queue_size=20,
+        )
+
+        # Print the entire state of the robot:
+        print("============ Printing robot state")
+        print(self.robot.get_current_state())
+        print("")
+
+        # Misc variables
+        self.box_name = ""
+
+        # Logging starting pose. There are better ways to do this.
+        self.arm_l_start = self.move_group_arm_l.get_current_pose().pose
+        self.arm_r_start = self.move_group_arm_r.get_current_pose().pose
+
+        self.rate = rospy.Rate(1.0)
+
+    def move_gripper(self, gripper='right', command='open'):
+        print()
+        print(f"=========={gripper} gripper will {command}...==========")
+
+        move_group = self.move_group_hand_r
+        if gripper == "left":
+            move_group = self.move_group_hand_l
+
+        joint_goal = move_group.get_current_joint_values()
+        print(f"gripper current joints:{joint_goal}")
+        joint_goal[0] = 0.5
+        joint_goal[1] = 0.0
+        joint_goal[2] = 0.5
+        joint_goal[3] = 0.0
+        if command == 'close':
+            close = 0.17453292519943295
+            # 0.1144640137963143
+            joint_goal[0] = close/10.0
+            joint_goal[2] = close
+
+        move_group.go(joint_goal, wait=True)
+        move_group.stop()
+        print(self.robot.get_current_state())
+
+    def move_arm(self, arm='right', joint_angles=[]):
+        print()
+        print(f"==========Moving {arm} to goal:{joint_angles}...==========")
+
+        if len(joint_angles) < 6:
+            print("Error: Fewer than 6 joint angles provided. Aborting...")
+            return
+
+        move_group = self.move_group_arm_r
+        if arm == 'left':
+            move_group = self.move_group_arm_l
+        # Call the planner to compute the plan and execute it.
+        plan = move_group.go(joint_angles, wait=True)
+        # Call `stop()` to ensure there is no residual movement
+        move_group.stop()
+        # Clear your targets after planning with poses.
+        move_group.clear_pose_targets()
+
+        print(self.robot.get_current_state())
+
+
+    def cartesian_move_z(self, move_group, z, scale=1):
+        waypoints = []
+        wpose = move_group.get_current_pose().pose
+        wpose.position.z += z
+        waypoints.append(copy.deepcopy(wpose))
+        (plan, fraction) = move_group.compute_cartesian_path(
+            waypoints, 0.01, 0.0)
+        move_group.execute(plan, wait=True)
+        # return plan, fraction
+
+def main():
+    try:
+        print("Setting up moveit commander")
+        bimanual_demo = MoveGroupPythonInterface()
+        bimanual_demo.move_gripper('left', 'close')
+        bimanual_demo.move_arm(
+          'left',
+          [0.2725458466968526, 0.3199954120186353, 0.27050573169303094, -0.05612724177451451, -0.3366234745727277, 0.27816477039994353])
+        bimanual_demo.move_gripper('left', 'open')
+        bimanual_demo.move_arm(
+          'left',
+          [0.2525493202907798, -0.20498309678016832, 0.8860128543728201, -0.026684396779588802, -0.4685281195659303, 0.25894079241234275])
+        bimanual_demo.move_gripper('left', 'close')
+        bimanual_demo.move_arm(
+          'left',
+          [0.22463184903468725,0.5640109055722806,-0.010189445663893758,-0.0029447975262430984,-0.40265756418968657,0.2046361271467092])
+        bimanual_demo.move_arm(
+          'left',
+          [-0.4396898837134099,0.5321104980211642,0.1817969793457925,0.2751617627062951,-0.5456892082990646,-0.7223044226272037])
+        bimanual_demo.move_arm(
+          'left',
+          [-0.5076864582614361, 0.6430632572402184, -0.1964135304533572, 0.4261104261543335, -0.31872944826848815, -1.014079334617769])
+        bimanual_demo.move_arm(
+          'left',
+          [-0.5605443400556704, 0.6194194950284646, -0.43562651809666936, -0.6993978537986612, 0.6179626637170913, 0.0903226598271187])
+
+    except rospy.ROSInterruptException:
+        return
+    except KeyboardInterrupt:
+        return
+
+if __name__ == "__main__":
+    main()

models/dave_object_models/models/electrical_panel/model.sdf

@@ -86,6 +86,7 @@
       <sensorPlateLink>m_to_f_sensor_plate</sensorPlateLink>
       <plugModel>m_to_f_plug</plugModel>
       <plugLink>m_to_f_plug</plugLink>
+      <matingForce>0.0</matingForce>
       <rollAlignmentTolerance>0.3</rollAlignmentTolerance>
       <pitchAlignmentTolerance>0.3</pitchAlignmentTolerance>
       <yawAlignmentTolerance>0.3</yawAlignmentTolerance>

models/dave_object_models/models/m_to_f_plug/model.sdf

@@ -24,6 +24,21 @@
             <scale>0.1 0.1 0.1</scale>
           </mesh>
         </geometry>
+        <surface>
+          <contact>
+            <ode>
+            <kp>1e6</kp>
+            <kd>1.0</kd>
+            <min_depth>0.001</min_depth>
+            </ode>
+          </contact>
+          <friction>
+            <ode>
+            <mu1>100.0</mu1>
+            <mu2>50.0</mu2>
+            </ode>
+          </friction>
+        </surface>
       </collision>
 
       <!-- <collision name ='stand'>
@@ -43,7 +58,7 @@
           <iyz>0</iyz>
           <izz>0.04</izz>
         </inertia>
-        <mass>0.5</mass>
+        <mass>0.2</mass>
       </inertial>
     </link>
 

models/dave_worlds/worlds/dave_integrated.world

@@ -163,7 +163,7 @@
     <!-- Electrical flying lead -->
     <include>
       <uri>model://m_to_f_plug</uri>
-      <pose>-191.06 117.25 -155.84 0 -1.570777 0</pose>
+      <pose>-189.80 114.41 -157.611077 0 -1.570777 0</pose>
     </include>
 
     <include>
@@ -173,7 +173,7 @@
 
     <model name="platform">
       <static>true</static>
-      <pose>-191.06 117.5 -156 0 0 0</pose>
+      <pose>-191.06 117.5 -155.8 0 0 0</pose>
       <link name="link">
         <collision name="collision">
           <geometry>

urdf/robots/rexrov_description/launch/upload_rexrov_oberon7_moveit.launch

@@ -6,9 +6,11 @@
 	<arg name="x" default="4"/>
 	<arg name="y" default="2"/>
 	<arg name="z" default="-91"/>
+	<arg name="roll" default="0.0"/>
+	<arg name="pitch" default="0.0"/>
 	<arg name="yaw" default="3.141592"/>
 
 	<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)"/>
 
-	<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"/>
-</launch>
+	<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"/>
+</launch>

urdf/robots/rexrov_oberon7_moveit/config/rexrov.srdf

@@ -192,6 +192,7 @@
     <disable_collisions link1="oberon7_l/finger_right" link2="rexrov/thruster_5" reason="Never"/>
     <disable_collisions link1="oberon7_l/finger_right" link2="rexrov/thruster_6" reason="Never"/>
     <disable_collisions link1="oberon7_l/finger_right" link2="rexrov/thruster_7" reason="Never"/>
+    <disable_collisions link1="oberon7_l/finger_right" link2="oberon7_l/wrist_link" reason="Never"/>
     <disable_collisions link1="oberon7_l/finger_tip_left" link2="oberon7_l/finger_tip_right" reason="Never"/>
     <disable_collisions link1="oberon7_l/finger_tip_left" link2="oberon7_l/forearm" reason="Never"/>
     <disable_collisions link1="oberon7_l/finger_tip_left" link2="oberon7_l/wrist_link" reason="Never"/>

urdf/robots/rexrov_oberon7_moveit/package.xml

@@ -24,8 +24,8 @@
   <run_depend>moveit_ros_visualization</run_depend>
   <run_depend>moveit_setup_assistant</run_depend>
   <run_depend>moveit_simple_controller_manager</run_depend>
-  <exec_depend>moveit_commander</exec_depend>
-  <exec_depend>moveit_planners</exec_depend>
+  <run_depend>moveit_commander</run_depend>
+  <run_depend>moveit_planners</run_depend>
   <run_depend>joint_state_publisher</run_depend>
   <run_depend>joint_state_publisher_gui</run_depend>
   <run_depend>robot_state_publisher</run_depend>