From c0d20e2f7bba91537917a0410e21de235c12c29d Mon Sep 17 00:00:00 2001 From: Thomas Muller Date: Tue, 28 Sep 2021 15:30:05 +0000 Subject: [PATCH] Kinda works... ish --- src/ar3_config/config/ar3_joy.yaml | 13 ++++--- src/ar3_config/launch/dev.launch.py | 1 + src/cobot_platform/cobot_platform/move.py | 8 ++-- src/joystickmove/joystickmove/move.py | 47 ++++++++++++++--------- 4 files changed, 41 insertions(+), 28 deletions(-) diff --git a/src/ar3_config/config/ar3_joy.yaml b/src/ar3_config/config/ar3_joy.yaml index 5e7bfa1..12ae0db 100644 --- a/src/ar3_config/config/ar3_joy.yaml +++ b/src/ar3_config/config/ar3_joy.yaml @@ -4,7 +4,7 @@ use_gazebo: false # Whether the robot is started in a Gazebo simulation environment ## Properties of incoming commands -command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. @@ -14,7 +14,7 @@ scale: ## Properties of outgoing commands publish_period: 0.034 # 1/Nominal publish rate [seconds] -low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) +low_latency_mode: true # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory @@ -37,14 +37,14 @@ ee_frame_name: link_6 # The name of the end effector link, used to return the E robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour -incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +incoming_command_timeout: 10.1 # Stop servoing if X seconds elapse without a new command # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. # Important because ROS may drop some messages and we need the robot to halt reliably. num_outgoing_halt_msgs_to_publish: 4 ## Configure handling of singularities and joint limits -lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) -hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this +lower_singularity_threshold: 170000.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 300000.0 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names @@ -53,9 +53,10 @@ joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle comm joint_topic: /joint_states status_topic: ~/status # Publish status to this topic command_out_topic: /joint_trajectory_controller/joint_trajectory # Publish outgoing commands here +# command_out_topic: /gaycunts # Publish outgoing commands here ## Collision checking for the entire robot body -check_collisions: true # Check collisions? +check_collisions: false # Check collisions? collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. # Two collision check algorithms are available: # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. diff --git a/src/ar3_config/launch/dev.launch.py b/src/ar3_config/launch/dev.launch.py index 74b71ab..872ceb2 100644 --- a/src/ar3_config/launch/dev.launch.py +++ b/src/ar3_config/launch/dev.launch.py @@ -63,6 +63,7 @@ def generate_launch_description(): {'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))}, robot_description, robot_description_semantic, + robot_description_kinematics, ], extra_arguments=[{"use_intra_process_comms": True}], ), diff --git a/src/cobot_platform/cobot_platform/move.py b/src/cobot_platform/cobot_platform/move.py index 5be1f39..eaef00f 100644 --- a/src/cobot_platform/cobot_platform/move.py +++ b/src/cobot_platform/cobot_platform/move.py @@ -54,11 +54,11 @@ class CobotMove(Node): print(self._serial.readline()) def _move_j1_cb(self, msg): - self._cur[0] = int(msg.data / pi / 2. * 8000) + self._cur[0] = int(msg.data / pi / 2. * 8000 * 4) self._move_servo() def _move_j2_cb(self, msg): - self._cur[1] = int(msg.data / pi / 2. * 10000) + self._cur[1] = int(msg.data / pi / 2. * 10000 * 4) self._move_servo() def _move_j3_cb(self, msg): @@ -84,8 +84,8 @@ class CobotMove(Node): line = self._serial.readline().decode('UTF-8') blocks = re.findall(r'[ABCDEF]-*\d+', line) - a = float(blocks[0][1:]) / 8000. * pi * 2. - b = float(blocks[1][1:]) / 10000 * pi * 2. + a = float(blocks[0][1:]) / 8000. / 4. * pi * 2. + b = float(blocks[1][1:]) / 10000 / 4. * pi * 2. c = float(blocks[2][1:]) / 10000. * pi * 2. d = float(blocks[3][1:]) / 2000. * pi * 2. e = float(blocks[4][1:]) / 2000. * pi * 2. diff --git a/src/joystickmove/joystickmove/move.py b/src/joystickmove/joystickmove/move.py index f305752..e396337 100644 --- a/src/joystickmove/joystickmove/move.py +++ b/src/joystickmove/joystickmove/move.py @@ -6,6 +6,7 @@ from std_srvs.srv import Trigger from std_msgs.msg import Float32 from sensor_msgs.msg import Joy from geometry_msgs.msg import TwistStamped +from control_msgs.msg import JointJog from moveit_msgs.msg import PlanningScene, PlanningSceneWorld from enum import Enum @@ -42,15 +43,16 @@ class JoyMove(Node): self._logger.info('joystick init') JOY_TOPIC = '/joy' - TWIST_TOPIC = '/servo_server/delta_twist_cmds' - # JOINT_TOPIC = '/servo_server/delta_joint_cmds' + # TWIST_TOPIC = '/servo_server/delta_twist_cmds' + JOINT_TOPIC = '/servo_server/delta_joint_cmds' # EE = 'joint_6' # BASE = 'base_link' self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data) - self.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10); - self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10); + # self.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10); + self.joint_pub = self.create_publisher(JointJog, JOINT_TOPIC, 10); + # self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10); self.servo_client = self.create_client(Trigger, '/servo_server/start_servo') # Check if the a service is available @@ -72,26 +74,35 @@ class JoyMove(Node): axes = msg.axes buttons = msg.buttons - ts = TwistStamped() + # ts = TwistStamped() + jj = JointJog() - ts.twist.linear.z = axes[Axes.RIGHT_Y.value] - ts.twist.linear.y = axes[Axes.RIGHT_X.value] + jj.joint_names = ['joint_2'] + # jj.displacements = [axes[0]] + jj.velocities = [axes[0]] + # jj.duration = 1.0 + # ts.twist.linear.z = axes[Axes.RIGHT_Y.value] + # ts.twist.linear.y = axes[Axes.RIGHT_X.value] - lin_x_right = -0.5 * axes[Axes.R1.value] # - AXIS_DEFAULTS.at(R1)); - lin_x_left = 0.5 * axes[Axes.L1.value] # - AXIS_DEFAULTS.at(L1)); - ts.twist.linear.x = lin_x_right + lin_x_left + # lin_x_right = -0.5 * axes[Axes.R1.value] # - AXIS_DEFAULTS.at(R1)); + # lin_x_left = 0.5 * axes[Axes.L1.value] # - AXIS_DEFAULTS.at(L1)); + # ts.twist.linear.x = lin_x_right + lin_x_left - ts.twist.angular.y = axes[Axes.LEFT_Y.value] - ts.twist.angular.x = axes[Axes.LEFT_X.value] + # ts.twist.angular.y = axes[Axes.LEFT_Y.value] + # ts.twist.angular.x = axes[Axes.LEFT_X.value] - roll_positive = buttons[Buttons.R2.value] - roll_negative = -1 * buttons[Buttons.L2.value] - ts.twist.angular.z = float(roll_positive + roll_negative) + # roll_positive = buttons[Buttons.R2.value] + # roll_negative = -1 * buttons[Buttons.L2.value] + # ts.twist.angular.z = float(roll_positive + roll_negative) - ts.header.stamp = self.get_clock().now().to_msg() - ts.header.frame_id = "base_link" + # ts.header.stamp = self.get_clock().now().to_msg() + # ts.header.frame_id = "base_link" - self.twist_pub.publish(ts) + jj.header.stamp = self.get_clock().now().to_msg() + jj.header.frame_id = "base_link" + + # self.twist_pub.publish(ts) + self.joint_pub.publish(jj) def main(args=None):