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