hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Execute grasps with MoveIt

Browse Source
This commit is contained in:
Michel Breyer 2021-09-04 15:50:29 +02:00
parent c508c65038
commit 3f3b58f404
7 changed files with 416 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
active_grasp/baselines.py
View File

@ -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]

65
active_grasp/controller.py
View File

@ -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

8
active_grasp/policy.py
View File

@ -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)

4
cfg/active_grasp.yaml
View File

@ -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

346
launch/active_grasp.rviz
View File

@ -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: <Fixed 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

8
launch/simulation.launch
View File

@ -2,12 +2,18 @@
<launch>
<arg name="launch_rviz" default="false" />
<!-- Load parameters -->
<rosparam command="load" file="$(find active_grasp)/cfg/active_grasp.yaml" subst_value="true" />
<!-- Launch simulated robot -->
<param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/urdfs/franka/panda_arm_hand.urdf.xacro" />
<node pkg="active_grasp" type="bt_sim_node.py" name="bt_sim" output="screen" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
<!-- Launch MoveIt -->
<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster" args="0 0 0 0 0 0 world panda_link0" />
<include file="$(find panda_moveit_config)/launch/move_group.launch" />
<!-- Launch rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/launch/active_grasp.rviz" if="$(arg launch_rviz)" />
</launch>

134
scripts/bt_sim_node.py
View File

@ -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)