Pengchen

.gitignore

@@ -0,0 +1 @@
+*.pyc

README.md

@@ -21,4 +21,17 @@ Welcome to the ROS driver for the Unitree Z1!
 
 **Attention:** The `z1_controller` package in `z1_ros` is different from [https://github.com/unitreerobotics/z1_controller](https://github.com/unitreerobotics/z1_controller), and [https://github.com/unitreerobotics/z1_sdk](https://github.com/unitreerobotics/z1_sdk) is not compatible with this package.
 
-See details [z1_controller](doc/controller.md)
+See details [z1_controller](doc/controller.md)
+
+
+## BRL Notes
+### Simulated Robot Arm
+Z1_README: [https://dev-z1.unitree.com/brief/installation.html](https://dev-z1.unitree.com/brief/installation.html)
+
+If you aren't using a real robot arm, you can simulate an arm in Gazebo.
+
+```bash
+roslaunch z1_bringup sim_arm.launch UnitreeGripperYN:=false RealSense:=true rviz:=true
+roslaunch z1_bringup real_arm.launch rviz:=true UnitreeGripperYN:=false RealSense:=false
+
+```

z1_bringup/launch/sim_arm.launch

@@ -1,5 +1,6 @@
 <launch>
   <arg name="UnitreeGripperYN" default="true"/>
+  <arg name="RealSense" default="true"/>
 
   <arg name="controller" default="true" />
   <arg name="ros_controller" default="true" />
@@ -8,8 +9,13 @@
 
   <include file="$(find z1_bringup)/launch/sim_ctrl.launch" if="$(arg controller)">
     <arg name="UnitreeGripperYN" value="$(arg UnitreeGripperYN)"/>
+    <!-- <arg name="RealSense" value="$(arg RealSense)"/> -->
   </include>
 
+  <group if="$(arg RealSense)">
+      <node name="image_proc" pkg="image_proc" type="image_proc" ns="camera/color"/>
+  </group>
+
   <group if="$(arg ros_controller)">
     <include file="$(find z1_hw)/launch/z1_hw.launch">
       <arg name="UnitreeGripper" value="$(arg UnitreeGripperYN)"/>

z1_bringup/launch/sim_ctrl.launch

@@ -1,8 +1,10 @@
 <launch>
   <arg name="UnitreeGripperYN" default="true"/>
+  <!-- <arg name="RealSense" default="true"/> -->
 
   <include file="$(find unitree_gazebo)/launch/z1.launch">
     <arg name="UnitreeGripperYN" value="$(arg UnitreeGripperYN)"/>
+    <!-- <arg name="RealSense" value="$(arg RealSense)"/> -->
   </include>
 
   <node pkg="z1_controller" type="z1_ctrl" name="z1_ctrl" output="screen">

z1_controller/config/config.yaml

@@ -6,7 +6,7 @@ udp:
 collision:
   # When a collision is detected, the robot arm will stop moving 
   # until the user executes `clearErrors()` in SDK
-  open: false
+  open: true
   limitT: 10
 
 # If cmdCache is true, z1_controller while execute ArmCmd.shift_command() 

z1_moveit_config/script/z1_servo_moveit_execute.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python
+"""
+Chen Peng
+"""
+import rospy
+import math
+import sys
+import geometry_msgs.msg
+import moveit_commander
+from moveit_commander.conversions import pose_to_list
+from std_msgs.msg import Float64MultiArray
+
+
+def all_close(goal, actual, tolerance):
+    """
+    Convenience method for testing if the values in two lists are within a tolerance of each other.
+    For Pose and PoseStamped inputs, the angle between the two quaternions is compared (the angle
+    between the identical orientations q and -q is calculated correctly).
+    @param: goal       A list of floats, a Pose or a PoseStamped
+    @param: actual     A list of floats, a Pose or a PoseStamped
+    @param: tolerance  A float
+    @returns: bool
+    """
+    if type(goal) is list:
+        for index in range(len(goal)):
+            if abs(actual[index] - goal[index]) > tolerance:
+                return False
+
+    elif type(goal) is geometry_msgs.msg.PoseStamped:
+        return all_close(goal.pose, actual.pose, tolerance)
+
+    elif type(goal) is geometry_msgs.msg.Pose:
+        x0, y0, z0, qx0, qy0, qz0, qw0 = pose_to_list(actual)
+        x1, y1, z1, qx1, qy1, qz1, qw1 = pose_to_list(goal)
+        # Euclidean distance
+        d = math.dist((x1, y1, z1), (x0, y0, z0))
+        # phi = angle between orientations
+        cos_phi_half = math.fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1)
+        return d <= tolerance and cos_phi_half >= math.cos(tolerance / 2.0)
+
+    return True
+
+
+class MoveitServoZ1(object):
+    def __init__(self, rate=30):
+        super(MoveitServoZ1, self).__init__()
+        ## First initialize `moveit_commander`_ and a `rospy`_ node:
+        moveit_commander.roscpp_initialize(sys.argv)
+        rospy.init_node("moveit_servo", anonymous=True, log_level=rospy.INFO)
+        self.namespace = rospy.get_namespace()
+        self.rate = rospy.Rate(rate)
+        rospy.loginfo(rospy.get_name() + " Start")
+        rospy.loginfo(rospy.get_name() + " Namespace: " + self.namespace)
+        self.create_move_group()
+        self.define_subscribers()
+        rospy.spin()
+
+    def define_subscribers(self):
+        # command dependent on the servo configure file
+        moveit_servo_command_topic = "/z1_joint_group_position_controller/command"
+        rospy.Subscriber(
+            moveit_servo_command_topic,
+            Float64MultiArray,
+            self.servo_command_callback,
+        )
+
+    def servo_command_callback(self, command: Float64MultiArray):
+        # execute the joint command from servo
+        # TODO: make sure this is preemptable
+        # self.go_to_joint_state(command.data)
+
+        # Calling ``stop()`` ensures that there is no residual movement
+        # self.move_group.stop()
+
+        # current_joints = self.move_group.get_current_joint_values()
+        # return all_close(joint_goal, current_joints, tolerance)
+        self.move_group.go(command.data, wait=False)
+        rospy.loginfo("go to joint states: " + str(command.data))
+
+    def create_move_group(self):
+        ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
+        ## kinematic model and the robot's current joint states
+        robot = moveit_commander.RobotCommander()
+        ## Instantiate a `PlanningSceneInterface`_ object.  This provides a remote interface
+        ## for getting, setting, and updating the robot's internal understanding of the
+        ## surrounding world:
+        scene = moveit_commander.PlanningSceneInterface()
+        ## Instantiate a `MoveGroupCommander`_ object.  This object is an interface
+        ## This interface can be used to plan and execute motions:
+        ## Make sure this is consistent with servo config
+        group_name = "manipulator"
+        move_group = moveit_commander.MoveGroupCommander(group_name)
+
+        ## Getting Basic Information
+        # We can get the name of the reference frame for this robot:
+        planning_frame = move_group.get_planning_frame()
+        print("[MoveGroup] Planning frame: %s" % planning_frame)
+        eef_link = move_group.get_end_effector_link()
+        print("[MoveGroup] End effector link: %s" % eef_link)
+        group_names = robot.get_group_names()
+        print("[MoveGroup] Available Planning Groups:", robot.get_group_names())
+        # print("[MoveGroup] Printing robot state")
+        # print(robot.get_current_state())
+
+        # Misc variables
+        self.robot = robot
+        self.scene = scene
+        self.move_group = move_group
+        self.planning_frame = planning_frame
+        self.eef_link = eef_link
+        self.group_names = group_names
+
+        # Set vel and acc scale
+        self.move_group.set_max_acceleration_scaling_factor(1)
+        self.move_group.set_max_velocity_scaling_factor(1)
+
+
+if __name__ == "__main__":
+    MoveitServoZ1()