Merge pull request Field-Robotics-Lab#217 from Field-Robotics-Lab/fea…

…ture/integrated_world Integrated World
hansolo12334 · Mar 26, 2022 · 916a5ad · 916a5ad
2 parents fa0fb8b + 85596b8
commit 916a5ad
Show file tree

Hide file tree

Showing 16 changed files with 1,139 additions and 28 deletions.
diff --git a/examples/dave_demo_launch/launch/dave_integrated_demo.launch b/examples/dave_demo_launch/launch/dave_integrated_demo.launch
@@ -0,0 +1,106 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="gui" default="true"/>
+    <arg name="paused" default="true"/>
+    <arg name="world_name" default="$(find dave_worlds)/worlds/dave_integrated.world"/>
+    <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">
+        <arg name="world_name" value="$(arg world_name)"/>
+        <arg name="paused" value="$(arg paused)"/>
+        <arg name="use_sim_time" value="true"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="headless" value="false"/>
+        <arg name="debug" value="$(arg debug)"/>
+        <arg name="verbose" value="false"/>
+    </include>
+
+
+    <!-- use ned frame north east down -->
+    <include file="$(find uuv_assistants)/launch/publish_world_ned_frame.launch"/>
+
+    <!-- timeout -->
+    <group if="$(arg set_timeout)">
+        <include file="$(find uuv_assistants)/launch/set_simulation_timer.launch">
+            <arg name="timeout" value="$(arg timeout)"/>
+        </include>
+    </group>
+
+    <!-- rexrov robot with oberon7 arm -->
+    <include file="$(find rexrov_description)/launch/upload_rexrov_oberon7.launch">
+        <arg name="namespace" value="rexrov0"/>
+        <arg name="x" value="7.087382"/>
+        <arg name="y" value="-8.720616"/>
+        <arg name="z" value="-116.360490"/>
+        <arg name="roll" value="0.011443"/>
+        <arg name="pitch" value="0.060502"/>
+        <arg name="yaw" value="0.304482"/>
+    </include>
+
+    <!-- Velocity teleop (UUV stays in position when joystick is not used) -->
+    <include if="$(arg velocity_control)" file="$(find uuv_control_cascaded_pid)/launch/joy_velocity.launch">
+        <arg name="uuv_name" value="$(arg namespace)" />
+        <arg name="model_name" value="rexrov" />
+        <arg name="joy_id" value="$(arg joy_id)"/>
+    </include>
+
+    <!-- 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="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.459771 -Y -1.57079632679 -unpause" respawn="false" output="screen" />
+
+</launch>
diff --git a/examples/dave_nodes/CMakeLists.txt b/examples/dave_nodes/CMakeLists.txt
@@ -37,6 +37,11 @@ install(DIRECTORY launch
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
         PATTERN "*~" EXCLUDE)
 
+# for config
+install(DIRECTORY config
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+        PATTERN "*~" EXCLUDE)
+
 # for Python scripts
 catkin_install_python(PROGRAMS
   src/simple_box_motion.py
@@ -45,4 +50,6 @@ catkin_install_python(PROGRAMS
   src/dvl_state_and_gradient_dsl.py
   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})
diff --git a/examples/dave_nodes/config/integrated_world_places.yaml b/examples/dave_nodes/config/integrated_world_places.yaml
@@ -0,0 +1,14 @@
+# Config for teleport script
+places:
+  home: {
+    'robot_pose': [7.087382,-8.720616,-116.360490,0.011443,0.060502,0.304482],
+    'camera_pose': [7.087382,-8.720616,-116.360490,0.011443,0.060502,0.304482]
+  }
+  artillery: {
+    'robot_pose': [-150.113998,97.727378,-155.246244,-0.011886,0.016902,1.687642],
+    'camera_pose': [-150.113998,97.727378,-155.246244,-0.011886,0.016902,1.687642]
+  }
+  mud: {
+    'robot_pose': [-66.099145,-208.337754,-145.292877,-0.010855,0.030645,-3.086439],
+    'camera_pose': [-66.099145,-208.337754,-145.292877,-0.010855,0.030645,-3.086439]
+  }
diff --git a/examples/dave_nodes/launch/integrated_world_teleporter.launch b/examples/dave_nodes/launch/integrated_world_teleporter.launch
@@ -0,0 +1,5 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="config" default="$(find dave_nodes)/config/integrated_world_places.yaml"/>
+    <node name="teleport_vehicle" pkg="dave_nodes" type="teleport_vehicle.py" args="-c $(arg config)" output="screen"/>
+</launch>
diff --git a/examples/dave_nodes/src/bimanual_integrated_world.py b/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()