From 3f3b58f404f0a8af6c748de7ef10629f4440c456 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Sat, 4 Sep 2021 15:50:29 +0200 Subject: [PATCH] Execute grasps with MoveIt --- active_grasp/baselines.py | 5 +- active_grasp/controller.py | 65 ++++--- active_grasp/policy.py | 8 +- cfg/active_grasp.yaml | 4 +- launch/active_grasp.rviz | 346 ++++++++++++++++++++++++++----------- launch/simulation.launch | 8 +- scripts/bt_sim_node.py | 134 +++++++++++--- 7 files changed, 416 insertions(+), 154 deletions(-) diff --git a/active_grasp/baselines.py b/active_grasp/baselines.py index 67246f6..d16e828 100644 --- a/active_grasp/baselines.py +++ b/active_grasp/baselines.py @@ -30,8 +30,9 @@ class TopTrajectory(MultiViewPolicy): def update(self, img, x): self.integrate(img, x) linear, angular = self.compute_error(self.x_d, x) - if np.linalg.norm(linear) < 0.01: + if np.linalg.norm(linear) < 0.02: self.done = True + return np.zeros(6) else: return self.compute_velocity_cmd(linear, angular) @@ -50,10 +51,10 @@ class CircularTrajectory(MultiViewPolicy): def update(self, img, x): self.integrate(img, x) - elapsed_time = (rospy.Time.now() - self.tic).to_sec() if elapsed_time > self.duration: self.done = True + return np.zeros(6) else: t = self.m(elapsed_time) eye = self.center + np.r_[self.r * np.cos(t), self.r * np.sin(t), self.h] diff --git a/active_grasp/controller.py b/active_grasp/controller.py index a3e602f..aff504a 100644 --- a/active_grasp/controller.py +++ b/active_grasp/controller.py @@ -1,3 +1,4 @@ +from controller_manager_msgs.srv import * import copy import cv_bridge from geometry_msgs.msg import Twist @@ -11,15 +12,16 @@ from active_grasp.srv import Reset, ResetRequest from robot_helpers.ros import tf from robot_helpers.ros.conversions import * from robot_helpers.ros.panda import PandaGripperClient +from robot_helpers.ros.moveit import MoveItClient from robot_helpers.spatial import Rotation, Transform class GraspController: def __init__(self, policy): self.policy = policy - self.reset_env = rospy.ServiceProxy("reset", Reset) self.load_parameters() self.lookup_transforms() + self.init_service_proxies() self.init_robot_connection() self.init_camera_stream() @@ -34,9 +36,28 @@ class GraspController: tf.init() self.T_ee_cam = tf.lookup(self.ee_frame, self.cam_frame) + def init_service_proxies(self): + self.reset_env = rospy.ServiceProxy("reset", Reset) + self.switch_controller = rospy.ServiceProxy( + "controller_manager/switch_controller", SwitchController + ) + def init_robot_connection(self): self.cartesian_vel_pub = rospy.Publisher("command", Twist, queue_size=10) self.gripper = PandaGripperClient() + self.moveit = MoveItClient("panda_arm") + + def switch_to_cartesian_velocity_control(self): + req = SwitchControllerRequest() + req.start_controllers = ["cartesian_velocity_controller"] + req.stop_controllers = ["position_joint_trajectory_controller"] + self.switch_controller(req) + + def switch_to_joint_trajectory_control(self): + req = SwitchControllerRequest() + req.start_controllers = ["position_joint_trajectory_controller"] + req.stop_controllers = ["cartesian_velocity_controller"] + self.switch_controller(req) def init_camera_stream(self): self.cv_bridge = cv_bridge.CvBridge() @@ -47,9 +68,15 @@ class GraspController: def run(self): bbox = self.reset() + + self.switch_to_cartesian_velocity_control() with Timer("search_time"): grasp = self.search_grasp(bbox) - res = self.execute_grasp(grasp) + + self.switch_to_joint_trajectory_control() + with Timer("execution_time"): + res = self.execute_grasp(grasp) + return self.collect_info(res) def reset(self): @@ -60,14 +87,11 @@ class GraspController: def search_grasp(self, bbox): self.policy.activate(bbox) r = rospy.Rate(self.policy.rate) - while True: + while not self.policy.done: img, pose = self.get_state() cmd = self.policy.update(img, pose) - if self.policy.done: - break - else: - self.cartesian_vel_pub.publish(to_twist_msg(cmd)) - r.sleep() + self.cartesian_vel_pub.publish(to_twist_msg(cmd)) + r.sleep() return self.policy.best_grasp def get_state(self): @@ -77,40 +101,25 @@ class GraspController: return img, pose def execute_grasp(self, grasp): - return if not grasp: return "aborted" T_base_grasp = self.postprocess(grasp.pose) + self.gripper.move(0.08) - # Move to an initial pose offset. - self.send_cmd( - T_base_grasp * Transform.translation([0, 0, -0.05]) * self.T_grasp_ee - ) - rospy.sleep(3.0) # TODO - - # Approach grasp pose. - self.send_cmd(T_base_grasp * self.T_grasp_ee) - rospy.sleep(2.0) - - # Close the fingers. + self.moveit.goto(T_base_grasp * Transform.t([0, 0, -0.05]) * self.T_grasp_ee) + self.moveit.goto(T_base_grasp * self.T_grasp_ee) self.gripper.grasp() + self.moveit.goto(Transform.t([0, 0, 0.2]) * T_base_grasp * self.T_grasp_ee) - # Lift the object. - target = Transform.translation([0, 0, 0.2]) * T_base_grasp * self.T_grasp_ee - self.send_cmd(target) - rospy.sleep(2.0) - - # Check whether the object remains in the hand success = self.gripper.read() > 0.005 return "succeeded" if success else "failed" def postprocess(self, T_base_grasp): - # Ensure that the camera is pointing forward. rot = T_base_grasp.rotation - if rot.as_matrix()[:, 0][0] < 0: + if rot.as_matrix()[:, 0][0] < 0: # Ensure that the camera is pointing forward T_base_grasp.rotation = rot * Rotation.from_euler("z", np.pi) return T_base_grasp diff --git a/active_grasp/policy.py b/active_grasp/policy.py index 748d94d..aca0e99 100644 --- a/active_grasp/policy.py +++ b/active_grasp/policy.py @@ -55,7 +55,10 @@ class Policy: return linear, angular def compute_velocity_cmd(self, linear, angular): - linear = linear / np.linalg.norm(linear) * self.linear_vel + kp = 4.0 + linear = kp * linear + scale = np.linalg.norm(linear) + linear *= np.clip(scale, 0.0, self.linear_vel) / scale return np.r_[linear, angular] def sort_grasps(self, in_grasps): @@ -85,7 +88,7 @@ class SingleViewPolicy(Policy): def update(self, img, x): linear, angular = self.compute_error(self.x_d, x) - if np.linalg.norm(linear) < 0.01: + if np.linalg.norm(linear) < 0.02: self.views.append(x) self.tsdf.integrate(img, self.intrinsic, x.inv() * self.T_base_task) @@ -103,6 +106,7 @@ class SingleViewPolicy(Policy): self.best_grasp = grasps[0] if len(grasps) > 0 else None self.done = True + return np.zeros(6) else: return self.compute_velocity_cmd(linear, angular) diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml index 4973be2..195aa23 100644 --- a/cfg/active_grasp.yaml +++ b/cfg/active_grasp.yaml @@ -6,8 +6,8 @@ bt_sim: grasp_controller: base_frame_id: panda_link0 ee_frame_id: panda_hand - ee_grasp_offset: [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.065] - linear_vel: 0.1 + ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065] # offset to the moveit ee + linear_vel: 0.05 qual_threshold: 0.9 camera: frame_id: camera_optical_frame diff --git a/launch/active_grasp.rviz b/launch/active_grasp.rviz index c4469a7..e74bbb2 100644 --- a/launch/active_grasp.rviz +++ b/launch/active_grasp.rviz @@ -7,7 +7,7 @@ Panels: - /TF1/Frames1 - /Markers1/Namespaces1 Splitter Ratio: 0.5 - Tree Height: 543 + Tree Height: 538 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -26,7 +26,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: SceneCloud + SyncSource: Image Preferences: PromptSaveOnExit: true Toolbars: @@ -132,39 +132,222 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: false + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: panda_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: false + Velocity_Scaling_Factor: 0.1 - Class: rviz/TF - Enabled: true + Enabled: false Frame Timeout: 15 Frames: All Enabled: false - camera_optical_frame: - Value: true - panda_hand: - Value: true - panda_leftfinger: - Value: false - panda_link0: - Value: true - panda_link1: - Value: false - panda_link2: - Value: false - panda_link3: - Value: false - panda_link4: - Value: false - panda_link5: - Value: false - panda_link6: - Value: false - panda_link7: - Value: false - panda_link8: - Value: false - panda_rightfinger: - Value: false - task: - Value: true Marker Alpha: 1 Marker Scale: 0.5 Name: TF @@ -172,37 +355,8 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - panda_link0: - panda_link1: - panda_link2: - panda_link3: - panda_link4: - panda_link5: - panda_link6: - panda_link7: - panda_link8: - panda_hand: - camera_optical_frame: - {} - panda_leftfinger: - {} - panda_rightfinger: - {} - task: - {} + {} Update Interval: 0 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /camera/depth/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false Value: false - Alpha: 1 Autocompute Intensity Bounds: true @@ -232,7 +386,7 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - - Alpha: 1 + - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 @@ -262,7 +416,7 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false - - Alpha: 0.20000000298023224 + - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 @@ -274,7 +428,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 138; 226; 52 Max Intensity: 1 @@ -291,36 +445,30 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: false - Value: true - - Alpha: 1 - Axes Length: 0.05000000074505806 - Axes Radius: 0.009999999776482582 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: EETarget - Queue Size: 10 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Axes - Topic: /command - Unreliable: false - Value: true + Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: visualization_marker_array Name: Markers Namespaces: bbox: true + best_grasp: true grasps: true path: true - point: true - rays: true - views: true Queue Size: 100 Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /camera/depth/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -349,7 +497,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.4553453922271729 + Distance: 1.1316771507263184 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -357,27 +505,31 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.43946000933647156 - Y: 0.016814040020108223 - Z: 0.40760287642478943 + X: 0.26778873801231384 + Y: 0.05089891329407692 + Z: 0.41754576563835144 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.19979602098464966 + Pitch: 0.27479735016822815 Target Frame: - Yaw: 5.38097620010376 + Yaw: 5.182796955108643 Saved: ~ Window Geometry: - Depth: - collapsed: false Displays: collapsed: false - Height: 762 + Height: 1019 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000025cfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000025c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0044006500700074006800000001e4000000b50000001600ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004000000003efc0100000002fb0000000800540069006d0065010000000000000400000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002a40000025c00000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Image: + collapsed: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f300000362fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000257000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a004400650070007400680000000166000000b50000000000000000fb0000000a0049006d006100670065010000029a000001050000001600fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000217000001880000017d00fffffffb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ec00000039fc0100000002fb0000000800540069006d00650100000000000005ec000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003f30000036200000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -386,6 +538,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1024 - X: 1440 - Y: 268 + Width: 1516 + X: 905 + Y: 139 diff --git a/launch/simulation.launch b/launch/simulation.launch index 69ed210..1894ed7 100644 --- a/launch/simulation.launch +++ b/launch/simulation.launch @@ -2,12 +2,18 @@ + + - + + + + + diff --git a/scripts/bt_sim_node.py b/scripts/bt_sim_node.py index 211a77f..58bf919 100755 --- a/scripts/bt_sim_node.py +++ b/scripts/bt_sim_node.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 from actionlib import SimpleActionServer +import control_msgs.msg +from controller_manager_msgs.srv import * import cv_bridge from franka_gripper.msg import * from geometry_msgs.msg import Twist @@ -27,34 +29,22 @@ class BtSimNode: self.plugins = [ PhysicsPlugin(self.sim), JointStatePlugin(self.sim.arm, self.sim.gripper), - CartesianVelocityControllerPlugin(self.sim.arm, self.sim.model), MoveActionPlugin(self.sim.gripper), GraspActionPlugin(self.sim.gripper), + GripperActionPlugin(), CameraPlugin(self.sim.camera), ] - - def advertise_services(self): - rospy.Service("seed", Seed, self.seed) - rospy.Service("reset", Reset, self.reset) - - def seed(self, req): - self.sim.seed(req.seed) - return SeedResponse() - - def reset(self, req): - self.deactivate_plugins() - rospy.sleep(1.0) # TODO replace with a read-write lock - bbox = self.sim.reset() - self.activate_plugins() - return ResetResponse(to_bbox_msg(bbox)) - - def run(self): - self.start_plugins() - self.activate_plugins() - rospy.spin() + self.controllers = { + "cartesian_velocity_controller": CartesianVelocityControllerPlugin( + self.sim.arm, self.sim.model + ), + "position_joint_trajectory_controller": JointTrajectoryControllerPlugin( + self.sim.arm + ), + } def start_plugins(self): - for plugin in self.plugins: + for plugin in self.plugins + list(self.controllers.values()): plugin.thread.start() def activate_plugins(self): @@ -65,6 +55,43 @@ class BtSimNode: for plugin in self.plugins: plugin.deactivate() + def deactivate_controllers(self): + for controller in self.controllers.values(): + controller.deactivate() + + def advertise_services(self): + rospy.Service("seed", Seed, self.seed) + rospy.Service("reset", Reset, self.reset) + rospy.Service( + "/controller_manager/switch_controller", + SwitchController, + self.switch_controller, + ) + + def seed(self, req): + self.sim.seed(req.seed) + return SeedResponse() + + def reset(self, req): + self.deactivate_plugins() + self.deactivate_controllers() + rospy.sleep(1.0) # TODO replace with a read-write lock + bbox = self.sim.reset() + self.activate_plugins() + return ResetResponse(to_bbox_msg(bbox)) + + def switch_controller(self, req): + for controller in req.stop_controllers: + self.controllers[controller].deactivate() + for controller in req.start_controllers: + self.controllers[controller].activate() + return SwitchControllerResponse(ok=True) + + def run(self): + self.start_plugins() + self.activate_plugins() + rospy.spin() + class Plugin: """A plugin that spins at a constant rate in its own thread.""" @@ -134,6 +161,11 @@ class CartesianVelocityControllerPlugin(Plugin): self.dx_d = np.zeros(6) self.is_running = True + def deactivate(self): + self.dx_d = np.zeros(6) + self.is_running = False + self.arm.set_desired_joint_velocities(np.zeros(7)) + def update(self): q, _ = self.arm.get_state() J_pinv = np.linalg.pinv(self.model.jacobian(q)) @@ -141,6 +173,41 @@ class CartesianVelocityControllerPlugin(Plugin): self.arm.set_desired_joint_velocities(cmd) +class JointTrajectoryControllerPlugin(Plugin): + def __init__(self, arm, rate=30): + super().__init__(rate) + self.arm = arm + self.dt = 1.0 / self.rate + self.init_action_server() + + def init_action_server(self): + name = "position_joint_trajectory_controller/follow_joint_trajectory" + self.action_server = SimpleActionServer( + name, + control_msgs.msg.FollowJointTrajectoryAction, + auto_start=False, + ) + self.action_server.register_goal_callback(self.action_goal_cb) + self.action_server.start() + + def action_goal_cb(self): + goal = self.action_server.accept_new_goal() + self.elapsed_time = 0.0 + self.points = iter(goal.trajectory.points) + self.next_point = next(self.points) + + def update(self): + if self.action_server.is_active(): + self.elapsed_time += self.dt + if self.elapsed_time > self.next_point.time_from_start.to_sec(): + try: + self.next_point = next(self.points) + except StopIteration: + self.action_server.set_succeeded() + return + self.arm.set_desired_joint_positions(self.next_point.positions) + + class MoveActionPlugin(Plugin): def __init__(self, gripper, rate=10): super().__init__(rate) @@ -191,6 +258,29 @@ class GraspActionPlugin(Plugin): self.action_server.set_succeeded() +class GripperActionPlugin(Plugin): + """Empty action server to make MoveIt happy""" + + def __init__(self, rate=1): + super().__init__(rate) + self.init_action_server() + + def init_action_server(self): + name = "/franka_gripper/gripper_action" + self.action_server = SimpleActionServer( + name, control_msgs.msg.GripperCommandAction, auto_start=False + ) + self.action_server.register_goal_callback(self.action_goal_cb) + self.action_server.start() + + def action_goal_cb(self): + self.action_server.accept_new_goal() + + def update(self): + if self.action_server.is_active(): + self.action_server.set_succeeded() + + class CameraPlugin(Plugin): def __init__(self, camera, name="camera", rate=5): super().__init__(rate)