def __init__(self):
gripper = rospy.get_param("~gripper", "panda")
self.curr_velocity_publish_rate = 100.0 # Hz
self.curr_velo_pub = rospy.Publisher(
"/cartesian_velocity_node_controller/cartesian_velocity",
Twist,
queue_size=1,
)
self.max_velo = 0.10
self.curr_velo = Twist()
self.best_grasp = Grasp()
self.cs = ControlSwitcher({
"moveit":
"position_joint_trajectory_controller",
"velocity":
"cartesian_velocity_node_controller",
})
self.cs.switch_controller("moveit")
self.pc = PandaCommander(group_name="panda_arm", gripper=gripper)
self.robot_state = None
self.ROBOT_ERROR_DETECTED = False
self.BAD_UPDATE = False
self.initial_pose = None
rospy.Subscriber(
"/franka_state_controller/franka_states",
FrankaState,
self.__robot_state_callback,
queue_size=1,
)
def __init__(self):
ggcnn_service_name = '/ggcnn_service'
rospy.wait_for_service(ggcnn_service_name + '/predict')
self.ggcnn_srv = rospy.ServiceProxy(ggcnn_service_name + '/predict',
GraspPrediction)
self.curr_velocity_publish_rate = 100.0 # Hz
self.curr_velo_pub = rospy.Publisher(
'/cartesian_velocity_node_controller/cartesian_velocity',
Twist,
queue_size=1)
self.max_velo = 0.10
self.curr_velo = Twist()
self.best_grasp = Grasp()
self.cs = ControlSwitcher({
'moveit':
'position_joint_trajectory_controller',
'velocity':
'cartesian_velocity_node_controller'
})
self.cs.switch_controller('moveit')
self.pc = PandaCommander(group_name='panda_arm_hand')
self.robot_state = None
self.ROBOT_ERROR_DETECTED = False
self.BAD_UPDATE = False
rospy.Subscriber('/franka_state_controller/franka_states',
FrankaState,
self.__robot_state_callback,
queue_size=1)
# Centre and above the bin
self.pregrasp_pose = [
(rospy.get_param('/grasp_entropy_node/histogram/bounds/x2') +
rospy.get_param('/grasp_entropy_node/histogram/bounds/x1')) / 2 -
0.03,
(rospy.get_param('/grasp_entropy_node/histogram/bounds/y2') +
rospy.get_param('/grasp_entropy_node/histogram/bounds/y1')) / 2 +
0.10,
rospy.get_param('/grasp_entropy_node/height/z1') + 0.05,
2**0.5 / 2, -2**0.5 / 2, 0, 0
]
self.last_weight = 0
self.__weight_increase_check()
self.experiment = Experiment()
def __init__(self):
gripper = rospy.get_param("~gripper", "panda")
if self.gripper == "panda":
self.LINK_EE_OFFSET = 0.1384
elif self.gripper == "robotiq":
self.LINK_EE_OFFSET = 0.30
ggrasp_service_name = "/ggrasp"
rospy.wait_for_service(ggrasp_service_name + "/predict")
self.ggrasp_srv = rospy.ServiceProxy(
ggrasp_service_name + "/predict", GraspPrediction
)
self.save_dir = "./ggrasp_data/"
self.depth_img = None
self.grasp = None
self.curr_velocity_publish_rate = 100.0 # Hz
self.curr_velo_pub = rospy.Publisher(
"/cartesian_velocity_node_controller/cartesian_velocity",
Twist,
queue_size=1,
)
self.max_velo = 0.10
self.curr_velo = Twist()
self.best_grasp = Grasp()
self.cs = ControlSwitcher(
{
"moveit": "position_joint_trajectory_controller",
"velocity": "cartesian_velocity_node_controller",
}
)
self.cs.switch_controller("moveit")
self.pc = PandaCommander(group_name="panda_arm", gripper=gripper)
self.initial_pose = None
self.robot_state = None
self.ROBOT_ERROR_DETECTED = False
self.BAD_UPDATE = False
rospy.Subscriber(
"/franka_state_controller/franka_states",
FrankaState,
self.__robot_state_callback,
queue_size=1,
)
def __init__(self):
self.gripper = rospy.get_param("~gripper", "panda")
self.curr_velocity_publish_rate = 100.0 # Hz
self.curr_velo_pub = rospy.Publisher(
"/cartesian_velocity_node_controller/cartesian_velocity",
Twist,
queue_size=1,
)
self.grasp_sub = rospy.Subscriber("/ggrasp/predict",
Grasp,
self.grasp_cmd_callback,
queue_size=1)
self.max_dist_to_target = 0.3 # distance to target to stop updating target pose
self.linear_velo = 0.05
self.curr_velo = Twist()
self.best_grasp = Grasp()
self.cs = ControlSwitcher({
"moveit":
"position_joint_trajectory_controller",
"velocity":
"cartesian_velocity_node_controller",
})
self.cs.switch_controller("velocity")
self.pc = PandaCommander(group_name="panda_arm", gripper=self.gripper)
self.robot_state = None
self.ROBOT_ERROR_DETECTED = False
self.BAD_UPDATE = False
rospy.Subscriber(
"/franka_state_controller/franka_states",
FrankaState,
self.__robot_state_callback,
queue_size=1,
)
def __init__(self):
entropy_node_name = "/grasp_entropy_node"
rospy.wait_for_service(entropy_node_name + "/update_grid")
self.entropy_srv = rospy.ServiceProxy(
entropy_node_name + "/update_grid", NextViewpoint)
rospy.wait_for_service(entropy_node_name + "/reset_grid")
self.entropy_reset_srv = rospy.ServiceProxy(
entropy_node_name + "/reset_grid", EmptySrv)
rospy.wait_for_service(entropy_node_name + "/add_failure_point")
self.add_failure_point_srv = rospy.ServiceProxy(
entropy_node_name + "/add_failure_point", AddFailurePoint)
self.curr_velocity_publish_rate = 100.0 # Hz
self.curr_velo_pub = rospy.Publisher(
"/cartesian_velocity_node_controller/cartesian_velocity",
Twist,
queue_size=1,
)
self.max_velo = 0.1
self.curr_velo = Twist()
self.best_grasp = Pose()
self.viewpoints = 0
self.grasp_width = 0.10
self._in_velo_loop = False
self.update_rate = 10.0 # Hz
update_topic_name = "~/update"
self.update_pub = rospy.Publisher(update_topic_name,
Empty,
queue_size=1)
rospy.Subscriber(update_topic_name,
Empty,
self.__update_callback,
queue_size=1)
self.cs = ControlSwitcher({
"moveit":
"position_joint_trajectory_controller",
"velocity":
"cartesian_velocity_node_controller",
})
self.cs.switch_controller("moveit")
self.pc = PandaCommander(group_name="panda_arm_hand")
self.robot_state = None
self.ROBOT_ERROR_DETECTED = False
self.BAD_UPDATE = False
rospy.Subscriber(
"/franka_state_controller/franka_states",
FrankaState,
self.__robot_state_callback,
queue_size=1,
)
# Centre and above the bin
self.pregrasp_pose = [
(rospy.get_param("/grasp_entropy_node/histogram/bounds/x2") +
rospy.get_param("/grasp_entropy_node/histogram/bounds/x1")) / 2,
(rospy.get_param("/grasp_entropy_node/histogram/bounds/y2") +
rospy.get_param("/grasp_entropy_node/histogram/bounds/y1")) / 2 +
0.08,
rospy.get_param("/grasp_entropy_node/height/z1") + 0.066,
2**0.5 / 2,
-(2**0.5) / 2,
0,
0,
]
self.last_weight = 0
self.__weight_increase_check()
self.experiment = Experiment()
class BaseGraspController(object):
"""
An base class for performing grasping experiments with the Panda robot.
"""
def __init__(self):
entropy_node_name = "/grasp_entropy_node"
rospy.wait_for_service(entropy_node_name + "/update_grid")
self.entropy_srv = rospy.ServiceProxy(
entropy_node_name + "/update_grid", NextViewpoint)
rospy.wait_for_service(entropy_node_name + "/reset_grid")
self.entropy_reset_srv = rospy.ServiceProxy(
entropy_node_name + "/reset_grid", EmptySrv)
rospy.wait_for_service(entropy_node_name + "/add_failure_point")
self.add_failure_point_srv = rospy.ServiceProxy(
entropy_node_name + "/add_failure_point", AddFailurePoint)
self.curr_velocity_publish_rate = 100.0 # Hz
self.curr_velo_pub = rospy.Publisher(
"/cartesian_velocity_node_controller/cartesian_velocity",
Twist,
queue_size=1,
)
self.max_velo = 0.1
self.curr_velo = Twist()
self.best_grasp = Pose()
self.viewpoints = 0
self.grasp_width = 0.10
self._in_velo_loop = False
self.update_rate = 10.0 # Hz
update_topic_name = "~/update"
self.update_pub = rospy.Publisher(update_topic_name,
Empty,
queue_size=1)
rospy.Subscriber(update_topic_name,
Empty,
self.__update_callback,
queue_size=1)
self.cs = ControlSwitcher({
"moveit":
"position_joint_trajectory_controller",
"velocity":
"cartesian_velocity_node_controller",
})
self.cs.switch_controller("moveit")
self.pc = PandaCommander(group_name="panda_arm_hand")
self.robot_state = None
self.ROBOT_ERROR_DETECTED = False
self.BAD_UPDATE = False
rospy.Subscriber(
"/franka_state_controller/franka_states",
FrankaState,
self.__robot_state_callback,
queue_size=1,
)
# Centre and above the bin
self.pregrasp_pose = [
(rospy.get_param("/grasp_entropy_node/histogram/bounds/x2") +
rospy.get_param("/grasp_entropy_node/histogram/bounds/x1")) / 2,
(rospy.get_param("/grasp_entropy_node/histogram/bounds/y2") +
rospy.get_param("/grasp_entropy_node/histogram/bounds/y1")) / 2 +
0.08,
rospy.get_param("/grasp_entropy_node/height/z1") + 0.066,
2**0.5 / 2,
-(2**0.5) / 2,
0,
0,
]
self.last_weight = 0
self.__weight_increase_check()
self.experiment = Experiment()
def __recover_robot_from_error(self):
rospy.logerr("Recovering")
self.pc.recover()
rospy.logerr("Done")
self.ROBOT_ERROR_DETECTED = False
def __weight_increase_check(self):
try:
w = rospy.wait_for_message("/scales/weight", Int16, timeout=2).data
increased = w > self.last_weight
self.last_weight = w
return increased
except:
return raw_input("No weight. Success? [1/0]") == "1"
def __robot_state_callback(self, msg):
self.robot_state = msg
if any(self.robot_state.cartesian_collision):
if not self.ROBOT_ERROR_DETECTED:
rospy.logerr("Detected Cartesian Collision")
self.ROBOT_ERROR_DETECTED = True
for s in FrankaErrors.__slots__:
if getattr(msg.current_errors, s):
self.stop()
if not self.ROBOT_ERROR_DETECTED:
rospy.logerr("Robot Error Detected")
self.ROBOT_ERROR_DETECTED = True
def __update_callback(self, msg):
# Update the MVP Controller asynchronously
if not self._in_velo_loop:
# Stop the callback lagging behind
return
res = self.entropy_srv()
if not res.success:
# Something has gone wrong, 0 velocity.
self.BAD_UPDATE = True
self.curr_velo = Twist()
return
self.viewpoints = res.no_viewpoints
# Calculate the required angular velocity to match the best grasp.
q = tfh.quaternion_to_list(res.best_grasp.pose.orientation)
curr_R = np.array(self.robot_state.O_T_EE).reshape((4, 4)).T
cpq = tft.quaternion_from_matrix(curr_R)
dq = tft.quaternion_multiply(q, tft.quaternion_conjugate(cpq))
d_euler = tft.euler_from_quaternion(dq)
res.velocity_cmd.angular.z = d_euler[2]
self.best_grasp = res.best_grasp
self.curr_velo = res.velocity_cmd
tfh.publish_pose_as_transform(self.best_grasp.pose, "panda_link0", "G",
0.05)
def __trigger_update(self):
# Let ROS handle the threading for me.
self.update_pub.publish(Empty())
def __velo_control_loop(self):
raise NotImplementedError()
def __execute_best_grasp(self):
self.cs.switch_controller("moveit")
# Offset for initial pose.
initial_offset = 0.05
LINK_EE_OFFSET = 0.138
# Add some limits, plus a starting offset.
self.best_grasp.pose.position.z = max(
self.best_grasp.pose.position.z - 0.01,
0.026) # 0.021 = collision with ground
self.best_grasp.pose.position.z += (
initial_offset + LINK_EE_OFFSET
) # Offset from end efector position to
self.pc.gripper.set_gripper(self.best_grasp.width, wait=False)
rospy.sleep(0.1)
self.pc.goto_pose(self.best_grasp.pose, velocity=0.5)
# Reset the position
self.best_grasp.pose.position.z -= initial_offset + LINK_EE_OFFSET
self.cs.switch_controller("velocity")
v = Twist()
v.linear.z = -0.05
# Monitor robot state and descend
while (self.robot_state.O_T_EE[-2] > self.best_grasp.pose.position.z
and not any(self.robot_state.cartesian_contact)
and not self.ROBOT_ERROR_DETECTED):
self.curr_velo_pub.publish(v)
rospy.sleep(0.01)
v.linear.z = 0
self.curr_velo_pub.publish(v)
# Check for collisions
if self.ROBOT_ERROR_DETECTED:
return False
# close the fingers.
rospy.sleep(0.2)
self.pc.gripper.grasp(0, force=2)
# Sometimes triggered by closing on something that pushes the robot
if self.ROBOT_ERROR_DETECTED:
return False
return True
def stop(self):
self.pc.stop()
self.curr_velo = Twist()
self.curr_velo_pub.publish(self.curr_velo)
def go(self):
raw_input("Press Enter to Start.")
while not rospy.is_shutdown():
self.cs.switch_controller("moveit")
self.pc.goto_named_pose("grip_ready", velocity=0.25)
start_pose = list(self.pregrasp_pose)
start_pose[0] += np.random.randn() * 0.05
start_pose[1] += np.random.randn() * 0.05
self.pc.goto_pose(start_pose, velocity=0.25)
self.pc.gripper.set_gripper(0.1)
self.cs.switch_controller("velocity")
self.entropy_reset_srv.call()
self.__trigger_update()
run = self.experiment.new_run()
run.start()
# Perform the velocity control portion.
self._in_velo_loop = True
velo_ok = self.__velo_control_loop()
self._in_velo_loop = False
if not velo_ok:
rospy.sleep(1.0)
if self.BAD_UPDATE:
raw_input("Fix Me! Enter to Continue")
self.BAD_UPDATE = False
if self.ROBOT_ERROR_DETECTED:
self.__recover_robot_from_error()
rospy.logerr("Aborting this Run.")
continue
# Execute the Grasp
grasp_ret = self.__execute_best_grasp()
run.stop()
if not grasp_ret or self.ROBOT_ERROR_DETECTED:
rospy.logerr("Something went wrong, aborting this run")
if self.ROBOT_ERROR_DETECTED:
self.__recover_robot_from_error()
continue
# Release Object
self.cs.switch_controller("moveit")
self.pc.goto_named_pose("grip_ready", velocity=0.5)
self.pc.goto_named_pose("drop_box", velocity=0.5)
self.pc.gripper.set_gripper(0.1)
# Check success using the scales.
rospy.sleep(1.0)
grasp_success = self.__weight_increase_check()
if not grasp_success:
rospy.logerr("Failed Grasp")
m = AddFailurePointRequest()
m.point.x = self.best_grasp.pose.position.x
m.point.y = self.best_grasp.pose.position.y
self.add_failure_point_srv.call(m)
else:
rospy.logerr("Successful Grasp")
run.success = grasp_success
run.quality = self.best_grasp.quality
run.entropy = self.best_grasp.entropy
run.viewpoints = self.viewpoints
run.save()
#! /usr/bin/env python
import rospy
from franka_control_wrappers.panda_commander import PandaCommander
import dougsm_helpers.tf_helpers as tfh
from dougsm_helpers.ros_control import ControlSwitcher
if __name__ == "__main__":
rospy.init_node("panda_open_loop_grasp")
gripper = rospy.get_param("~gripper", "panda")
cs = ControlSwitcher({
"moveit": "position_joint_trajectory_controller",
"velocity": "cartesian_velocity_node_controller",
})
cs.switch_controller("moveit")
pc = PandaCommander(group_name="panda_arm", gripper=gripper)
pc.print_debug_info()
print(pc.active_group.get_current_joint_values())
class PandaClosedLoopGraspController(object):
"""
Perform closed-loop grasps from a single viewpoint using the Panda robot.
"""
def __init__(self):
self.gripper = rospy.get_param("~gripper", "panda")
self.curr_velocity_publish_rate = 100.0 # Hz
self.curr_velo_pub = rospy.Publisher(
"/cartesian_velocity_node_controller/cartesian_velocity",
Twist,
queue_size=1,
)
self.grasp_sub = rospy.Subscriber("/ggrasp/predict",
Grasp,
self.grasp_cmd_callback,
queue_size=1)
self.max_dist_to_target = 0.3 # distance to target to stop updating target pose
self.linear_velo = 0.05
self.curr_velo = Twist()
self.best_grasp = Grasp()
self.cs = ControlSwitcher({
"moveit":
"position_joint_trajectory_controller",
"velocity":
"cartesian_velocity_node_controller",
})
self.cs.switch_controller("velocity")
self.pc = PandaCommander(group_name="panda_arm", gripper=self.gripper)
self.robot_state = None
self.ROBOT_ERROR_DETECTED = False
self.BAD_UPDATE = False
rospy.Subscriber(
"/franka_state_controller/franka_states",
FrankaState,
self.__robot_state_callback,
queue_size=1,
)
def grasp_cmd_callback(self, msg):
best_grasp = msg
tfh.publish_pose_as_transform(best_grasp.pose, "panda_link0", "G", 0.5)
# Rotate quaternion by 45 deg on the z axis to account for home position being -45deg
q_rot = tft.quaternion_from_euler(0, 0, np.pi / 4)
q_new = tfh.list_to_quaternion(
tft.quaternion_multiply(
tfh.quaternion_to_list(best_grasp.pose.orientation), q_rot))
best_grasp.pose.orientation = q_new
self.best_grasp = best_grasp
def __recover_robot_from_error(self):
rospy.logerr("Recovering")
self.pc.recover()
self.cs.switch_controller("moveit")
self.pc.goto_saved_pose("start", velocity=0.1)
rospy.logerr("Done")
self.ROBOT_ERROR_DETECTED = False
def __robot_state_callback(self, msg):
self.robot_state = msg
if any(self.robot_state.cartesian_collision):
if not self.ROBOT_ERROR_DETECTED:
rospy.logerr("Detected Cartesian Collision")
self.ROBOT_ERROR_DETECTED = True
for s in FrankaErrors.__slots__:
if getattr(msg.current_errors, s):
self.stop()
if not self.ROBOT_ERROR_DETECTED:
rospy.logerr("Robot Error Detected")
self.ROBOT_ERROR_DETECTED = True
def dist_to_target(self, target_grasp):
if target_grasp is None:
return 100000 # large number when there is no target
target_pose = target_grasp.pose
current_pose = tfh.current_robot_pose("world", "panda_EE")
x = target_pose.position.x - current_pose.position.x
y = target_pose.position.y - current_pose.position.y
z = target_pose.position.z - current_pose.position.z
return np.sqrt(x**2 + y**2 + z**2)
def get_velocity(self, target_pose):
"""Returns the distance from the target grasp from the current pose."""
current_pose = tfh.current_robot_pose("world", "panda_EE")
v = Twist()
v.linear.x = target_pose.position.x - current_pose.position.x
v.linear.y = target_pose.position.y - current_pose.position.y
v.linear.z = target_pose.position.z - current_pose.position.z
# v.angular.x,y = 0
current_euler = tft.euler_from_quaternion(
tfh.quaternion_to_list(current_pose.orientation))
target_euler = tft.euler_from_quaternion(
tfh.quaternion_to_list(target_pose.orientation))
v.angular.z = target_euler[2] - current_euler[2]
scaling_factor = self.linear_velo / np.sqrt(v.linear.x**2 +
v.linear.y**2 +
v.linear.z**2)
v.linear.x = scaling_factor * v.linear.x
v.linear.y = scaling_factor * v.linear.y
v.linear.z = scaling_factor * v.linear.z
v.angular.z = v.angular.z
return v
def __execute_grasp(self):
target_grasp = None
dist_to_target = self.dist_to_target(target_grasp)
gripper_width_offset = 0.01
while (self.robot_state.O_T_EE[-2] > self.best_grasp.pose.position.z
and not any(self.robot_state.cartesian_contact)
and not self.ROBOT_ERROR_DETECTED and dist_to_target > 0.01):
if not self.best_grasp:
break
if dist_to_target > self.max_dist_to_target:
target_grasp = self.best_grasp
target_grasp.pose.position.z += 0.05
v = self.get_velocity(target_grasp.pose)
self.curr_velo_pub.publish(v)
self.pc.gripper.set_gripper(target_grasp.width +
gripper_width_offset)
dist_to_target = self.dist_to_target(target_grasp)
rospy.sleep(0.01)
# Check for collisions
if self.ROBOT_ERROR_DETECTED:
return False
target_grasp.pose.position.z -= 0.05
while (not any(self.robot_state.cartesian_contact)
and not self.ROBOT_ERROR_DETECTED
and self.dist_to_target(target_grasp) > 0.01):
v = self.get_velocity(target_grasp.pose)
self.curr_velo_pub.publish(v)
rospy.sleep(0.01)
# Check for collisions
if self.ROBOT_ERROR_DETECTED:
return False
rospy.sleep(1)
self.cs.switch_controller("moveit")
# close the fingers.
rospy.sleep(0.2)
self.pc.gripper.grasp(0, force=1)
# Sometimes triggered by closing on something that pushes the robot
if self.ROBOT_ERROR_DETECTED:
return False
return True
def stop(self):
self.pc.stop()
self.curr_velo = Twist()
self.curr_velo_pub.publish(self.curr_velo)
def go(self):
self.cs.switch_controller("moveit")
self.pc.goto_saved_pose("start", velocity=0.1)
self.pc.gripper.set_gripper(0.1)
self.cs.switch_controller("velocity")
grasp_ret = self.__execute_grasp()
if not grasp_ret or self.ROBOT_ERROR_DETECTED:
rospy.logerr("Something went wrong, aborting this run")
if self.ROBOT_ERROR_DETECTED:
self.__recover_robot_from_error()
self.pc.goto_saved_pose("bin", velocity=0.1)
self.pc.gripper.set_gripper(0.1)
self.pc.goto_saved_pose("start", velocity=0.1)
class PandaOpenLoopGraspController(object):
"""
Perform open-loop grasps from a single viewpoint using the Panda robot.
"""
def __init__(self):
ggcnn_service_name = '/ggcnn_service'
rospy.wait_for_service(ggcnn_service_name + '/predict')
self.ggcnn_srv = rospy.ServiceProxy(ggcnn_service_name + '/predict',
GraspPrediction)
self.curr_velocity_publish_rate = 100.0 # Hz
self.curr_velo_pub = rospy.Publisher(
'/cartesian_velocity_node_controller/cartesian_velocity',
Twist,
queue_size=1)
self.max_velo = 0.10
self.curr_velo = Twist()
self.best_grasp = Grasp()
self.cs = ControlSwitcher({
'moveit':
'position_joint_trajectory_controller',
'velocity':
'cartesian_velocity_node_controller'
})
self.cs.switch_controller('moveit')
self.pc = PandaCommander(group_name='panda_arm_hand')
self.robot_state = None
self.ROBOT_ERROR_DETECTED = False
self.BAD_UPDATE = False
rospy.Subscriber('/franka_state_controller/franka_states',
FrankaState,
self.__robot_state_callback,
queue_size=1)
# Centre and above the bin
self.pregrasp_pose = [
(rospy.get_param('/grasp_entropy_node/histogram/bounds/x2') +
rospy.get_param('/grasp_entropy_node/histogram/bounds/x1')) / 2 -
0.03,
(rospy.get_param('/grasp_entropy_node/histogram/bounds/y2') +
rospy.get_param('/grasp_entropy_node/histogram/bounds/y1')) / 2 +
0.10,
rospy.get_param('/grasp_entropy_node/height/z1') + 0.05,
2**0.5 / 2, -2**0.5 / 2, 0, 0
]
self.last_weight = 0
self.__weight_increase_check()
self.experiment = Experiment()
def __recover_robot_from_error(self):
rospy.logerr('Recovering')
self.pc.recover()
rospy.logerr('Done')
self.ROBOT_ERROR_DETECTED = False
def __weight_increase_check(self):
try:
w = rospy.wait_for_message('/scales/weight', Int16, timeout=2).data
increased = w > self.last_weight
self.last_weight = w
return increased
except:
return raw_input('No weight. Success? [1/0]') == '1'
def __robot_state_callback(self, msg):
self.robot_state = msg
if any(self.robot_state.cartesian_collision):
if not self.ROBOT_ERROR_DETECTED:
rospy.logerr('Detected Cartesian Collision')
self.ROBOT_ERROR_DETECTED = True
for s in FrankaErrors.__slots__:
if getattr(msg.current_errors, s):
self.stop()
if not self.ROBOT_ERROR_DETECTED:
rospy.logerr('Robot Error Detected')
self.ROBOT_ERROR_DETECTED = True
def __execute_best_grasp(self):
self.cs.switch_controller('moveit')
ret = self.ggcnn_srv.call()
if not ret.success:
return False
best_grasp = ret.best_grasp
self.best_grasp = best_grasp
tfh.publish_pose_as_transform(best_grasp.pose, 'panda_link0', 'G', 0.5)
if raw_input('Continue?') == '0':
return False
# Offset for initial pose.
initial_offset = 0.10
LINK_EE_OFFSET = 0.138
# Add some limits, plus a starting offset.
best_grasp.pose.position.z = max(
best_grasp.pose.position.z - 0.01,
0.026) # 0.021 = collision with ground
best_grasp.pose.position.z += initial_offset + LINK_EE_OFFSET # Offset from end efector position to
self.pc.set_gripper(best_grasp.width, wait=False)
rospy.sleep(0.1)
self.pc.goto_pose(best_grasp.pose, velocity=0.1)
# Reset the position
best_grasp.pose.position.z -= initial_offset + LINK_EE_OFFSET
self.cs.switch_controller('velocity')
v = Twist()
v.linear.z = -0.05
# Monitor robot state and descend
while self.robot_state.O_T_EE[
-2] > best_grasp.pose.position.z and not any(
self.robot_state.cartesian_contact
) and not self.ROBOT_ERROR_DETECTED:
self.curr_velo_pub.publish(v)
rospy.sleep(0.01)
v.linear.z = 0
self.curr_velo_pub.publish(v)
# Check for collisions
if self.ROBOT_ERROR_DETECTED:
return False
# close the fingers.
rospy.sleep(0.2)
self.pc.grasp(0, force=2)
# Sometimes triggered by closing on something that pushes the robot
if self.ROBOT_ERROR_DETECTED:
return False
return True
def stop(self):
self.pc.stop()
self.curr_velo = Twist()
self.curr_velo_pub.publish(self.curr_velo)
def go(self):
raw_input('Press Enter to Start.')
while not rospy.is_shutdown():
self.cs.switch_controller('moveit')
self.pc.goto_named_pose('grip_ready', velocity=0.25)
self.pc.goto_pose(self.pregrasp_pose, velocity=0.25)
self.pc.set_gripper(0.1)
self.cs.switch_controller('velocity')
run = self.experiment.new_run()
run.start()
grasp_ret = self.__execute_best_grasp()
run.stop()
if not grasp_ret or self.ROBOT_ERROR_DETECTED:
rospy.logerr('Something went wrong, aborting this run')
if self.ROBOT_ERROR_DETECTED:
self.__recover_robot_from_error()
continue
# Release Object
self.cs.switch_controller('moveit')
self.pc.goto_named_pose('grip_ready', velocity=0.5)
self.pc.goto_named_pose('drop_box', velocity=0.5)
self.pc.set_gripper(0.07)
# Check success using the scales.
rospy.sleep(1.0)
grasp_success = self.__weight_increase_check()
if not grasp_success:
rospy.logerr("Failed Grasp")
else:
rospy.logerr("Successful Grasp")
run.success = grasp_success
run.quality = self.best_grasp.quality
run.save()
class PandaOpenLoopGraspController(object):
"""
Perform open-loop grasps from a single viewpoint using the Panda robot.
"""
def __init__(self):
gripper = rospy.get_param("~gripper", "panda")
self.curr_velocity_publish_rate = 100.0 # Hz
self.curr_velo_pub = rospy.Publisher(
"/cartesian_velocity_node_controller/cartesian_velocity",
Twist,
queue_size=1,
)
self.max_velo = 0.10
self.curr_velo = Twist()
self.best_grasp = Grasp()
self.cs = ControlSwitcher({
"moveit":
"position_joint_trajectory_controller",
"velocity":
"cartesian_velocity_node_controller",
})
self.cs.switch_controller("moveit")
self.pc = PandaCommander(group_name="panda_arm", gripper=gripper)
self.robot_state = None
self.ROBOT_ERROR_DETECTED = False
self.BAD_UPDATE = False
self.initial_pose = None
rospy.Subscriber(
"/franka_state_controller/franka_states",
FrankaState,
self.__robot_state_callback,
queue_size=1,
)
def __recover_robot_from_error(self):
rospy.logerr("Recovering")
self.pc.recover()
self.cs.switch_controller("moveit")
self.pc.goto_pose(self.initial_pose, velocity=0.1)
rospy.logerr("Done")
self.ROBOT_ERROR_DETECTED = False
def __robot_state_callback(self, msg):
self.robot_state = msg
if any(self.robot_state.cartesian_collision):
if not self.ROBOT_ERROR_DETECTED:
rospy.logerr("Detected Cartesian Collision")
self.ROBOT_ERROR_DETECTED = True
for s in FrankaErrors.__slots__:
if getattr(msg.current_errors, s):
self.stop()
if not self.ROBOT_ERROR_DETECTED:
rospy.logerr("Robot Error Detected")
self.ROBOT_ERROR_DETECTED = True
def stop(self):
self.pc.stop()
self.curr_velo = Twist()
self.curr_velo_pub.publish(self.curr_velo)
def go(self):
self.initial_pose = self.pc.get_current_pose()
self.cs.switch_controller("moveit")
new_pose = self.pc.get_current_pose()
new_pose.position.z += 0.05
self.pc.goto_pose(new_pose, velocity=0.1)
rospy.sleep(2)
self.pc.goto_pose(self.initial_pose, velocity=0.1)
self.pc.gripper.set_gripper(-0.01)
rospy.sleep(2)
self.pc.gripper.set_gripper(0.1)
class PandaCollectController(object):
"""
Perform open-loop grasps from a single viewpoint using the Panda robot.
Collect datapoints based on whether grasps are successful.
"""
def __init__(self):
gripper = rospy.get_param("~gripper", "panda")
if self.gripper == "panda":
self.LINK_EE_OFFSET = 0.1384
elif self.gripper == "robotiq":
self.LINK_EE_OFFSET = 0.30
ggrasp_service_name = "/ggrasp"
rospy.wait_for_service(ggrasp_service_name + "/predict")
self.ggrasp_srv = rospy.ServiceProxy(
ggrasp_service_name + "/predict", GraspPrediction
)
self.save_dir = "./ggrasp_data/"
self.depth_img = None
self.grasp = None
self.curr_velocity_publish_rate = 100.0 # Hz
self.curr_velo_pub = rospy.Publisher(
"/cartesian_velocity_node_controller/cartesian_velocity",
Twist,
queue_size=1,
)
self.max_velo = 0.10
self.curr_velo = Twist()
self.best_grasp = Grasp()
self.cs = ControlSwitcher(
{
"moveit": "position_joint_trajectory_controller",
"velocity": "cartesian_velocity_node_controller",
}
)
self.cs.switch_controller("moveit")
self.pc = PandaCommander(group_name="panda_arm", gripper=gripper)
self.initial_pose = None
self.robot_state = None
self.ROBOT_ERROR_DETECTED = False
self.BAD_UPDATE = False
rospy.Subscriber(
"/franka_state_controller/franka_states",
FrankaState,
self.__robot_state_callback,
queue_size=1,
)
def __recover_robot_from_error(self):
rospy.logerr("Recovering")
self.pc.recover()
self.cs.switch_controller("moveit")
self.pc.goto_pose(self.initial_pose, velocity=0.1)
rospy.logerr("Done")
self.ROBOT_ERROR_DETECTED = False
def __robot_state_callback(self, msg):
self.robot_state = msg
if any(self.robot_state.cartesian_collision):
if not self.ROBOT_ERROR_DETECTED:
rospy.logerr("Detected Cartesian Collision")
self.ROBOT_ERROR_DETECTED = True
for s in FrankaErrors.__slots__:
if getattr(msg.current_errors, s):
self.stop()
if not self.ROBOT_ERROR_DETECTED:
rospy.logerr("Robot Error Detected")
self.ROBOT_ERROR_DETECTED = True
def __execute_best_grasp(self):
self.cs.switch_controller("moveit")
ret = self.ggrasp_srv.call()
if not ret.success:
return False
self.best_grasp = ret.best_grasp
best_grasp = self.best_grasp
self.depth_img = bridge.imgmsg_to_cv2(ret.depth)
self.grasp = ret.grasp
tfh.publish_pose_as_transform(self.best_grasp.pose, "panda_link0", "G", 0.5)
# Rotate quaternion by 45 deg on the z axis to account for home position being -45deg
q_rot = tft.quaternion_from_euler(0, 0, np.pi / 4)
q_new = tfh.list_to_quaternion(
tft.quaternion_multiply(
tfh.quaternion_to_list(best_grasp.pose.orientation), q_rot
)
)
best_grasp.pose.orientation = q_new
# Offset for initial pose.
offset = 0.05 + self.LINK_EE_OFFSET
gripper_width_offset = 0.03
best_grasp.pose.position.z += offset
self.pc.gripper.set_gripper(best_grasp.width + gripper_width_offset, wait=False)
rospy.sleep(0.1)
self.pc.goto_pose(best_grasp.pose, velocity=0.1)
# Reset the position
best_grasp.pose.position.z -= offset
self.cs.switch_controller("velocity")
v = Twist()
v.linear.z = -0.05
# Monitor robot state and descend
while (
self.robot_state.O_T_EE[-2] > best_grasp.pose.position.z
and not any(self.robot_state.cartesian_contact)
and not self.ROBOT_ERROR_DETECTED
):
self.curr_velo_pub.publish(v)
rospy.sleep(0.01)
# Check for collisions
if self.ROBOT_ERROR_DETECTED:
return False
rospy.sleep(1)
self.cs.switch_controller("moveit")
# close the fingers.
rospy.sleep(0.2)
self.pc.gripper.grasp(0, force=1)
self.pc.goto_pose(self.initial_pose, velocity=0.1)
# Sometimes triggered by closing on something that pushes the robot
if self.ROBOT_ERROR_DETECTED:
return False
return True
def stop(self):
self.pc.stop()
self.curr_velo = Twist()
self.curr_velo_pub.publish(self.curr_velo)
def go(self):
self.initial_pose = self.pc.get_current_pose()
self.cs.switch_controller("moveit")
self.pc.gripper.set_gripper(0.1)
self.cs.switch_controller("velocity")
grasp_ret = self.__execute_best_grasp()
if not grasp_ret or self.ROBOT_ERROR_DETECTED:
rospy.logerr("Something went wrong, aborting this run")
if self.ROBOT_ERROR_DETECTED:
self.__recover_robot_from_error()
self.pc.gripper.set_gripper(0.1)
successful = raw_input("grasp successful? (y/n): ")
if successful == "y":
UID = uuid.uuid1()
cv2.imwrite(self.save_dir + str(UID) + ".png", self.depth_img)
with open(self.save_dir + str(UID) + ".grasp", "w") as f:
f.write(";".join([str(x) for x in self.grasp]))
class PandaOpenLoopGraspController(object):
"""
Perform open-loop grasps from a single viewpoint using the Panda robot.
"""
def __init__(self):
self.gripper = rospy.get_param("~gripper", "panda")
if self.gripper == "panda":
self.LINK_EE_OFFSET = 0.1384
elif self.gripper == "robotiq":
self.LINK_EE_OFFSET = 0.32
self.curr_velocity_publish_rate = 100.0 # Hz
self.curr_velo_pub = rospy.Publisher(
"/cartesian_velocity_node_controller/cartesian_velocity",
Twist,
queue_size=1,
)
self.max_velo = 0.10
self.curr_velo = Twist()
self.best_grasp = Grasp()
self.cs = ControlSwitcher({
"moveit":
"position_joint_trajectory_controller",
"velocity":
"cartesian_velocity_node_controller",
})
self.cs.switch_controller("moveit")
self.pc = PandaCommander(group_name="panda_arm", gripper=self.gripper)
self.robot_state = None
self.ROBOT_ERROR_DETECTED = False
self.BAD_UPDATE = False
rospy.Subscriber(
"/franka_state_controller/franka_states",
FrankaState,
self.__robot_state_callback,
queue_size=1,
)
def __recover_robot_from_error(self):
rospy.logerr("Recovering")
self.pc.recover()
self.cs.switch_controller("moveit")
self.pc.goto_saved_pose("start", velocity=0.1)
rospy.logerr("Done")
self.ROBOT_ERROR_DETECTED = False
def __robot_state_callback(self, msg):
self.robot_state = msg
if any(self.robot_state.cartesian_collision):
if not self.ROBOT_ERROR_DETECTED:
rospy.logerr("Detected Cartesian Collision")
self.ROBOT_ERROR_DETECTED = True
for s in FrankaErrors.__slots__:
if getattr(msg.current_errors, s):
self.stop()
if not self.ROBOT_ERROR_DETECTED:
rospy.logerr("Robot Error Detected")
self.ROBOT_ERROR_DETECTED = True
def __execute_best_grasp(self):
self.cs.switch_controller("moveit")
self.best_grasp = rospy.wait_for_message("/ggrasp/predict", Grasp)
self.best_grasp = correct_grasp(self.best_grasp, self.gripper)
tfh.publish_pose_as_transform(self.best_grasp.pose, "panda_link0", "G",
0.5)
# Offset for initial pose.
offset = 0.05 + self.LINK_EE_OFFSET
gripper_width_offset = 0.03
self.best_grasp.pose.position.z += offset
self.pc.gripper.set_gripper(self.best_grasp.width +
gripper_width_offset,
wait=False)
rospy.sleep(0.1)
self.pc.goto_pose(self.best_grasp.pose, velocity=0.1)
# Reset the position
self.best_grasp.pose.position.z -= offset
self.cs.switch_controller("velocity")
v = Twist()
v.linear.z = -0.05
# Monitor robot state and descend
while (self.robot_state.O_T_EE[-2] > self.best_grasp.pose.position.z
and not any(self.robot_state.cartesian_contact)
and not self.ROBOT_ERROR_DETECTED):
self.curr_velo_pub.publish(v)
rospy.sleep(0.01)
# Check for collisions
if self.ROBOT_ERROR_DETECTED:
return False
rospy.sleep(1)
self.cs.switch_controller("moveit")
# close the fingers.
rospy.sleep(0.2)
self.pc.gripper.grasp(0, force=1)
# Sometimes triggered by closing on something that pushes the robot
if self.ROBOT_ERROR_DETECTED:
return False
return True
def stop(self):
self.pc.stop()
self.curr_velo = Twist()
self.curr_velo_pub.publish(self.curr_velo)
def go(self):
self.cs.switch_controller("moveit")
self.pc.goto_saved_pose("start", velocity=0.1)
self.pc.gripper.set_gripper(0.1)
grasp_ret = self.__execute_best_grasp()
if not grasp_ret or self.ROBOT_ERROR_DETECTED:
rospy.logerr("Something went wrong, aborting this run")
if self.ROBOT_ERROR_DETECTED:
self.__recover_robot_from_error()
self.cs.switch_controller("moveit")
self.pc.goto_saved_pose("bin", velocity=0.1)
self.cs.switch_controller("velocity")
v = Twist()
v.linear.z = -0.05
# Monitor robot state and descend
while (self.robot_state.O_T_EE[-2] > self.best_grasp.pose.position.z
and not any(self.robot_state.cartesian_contact)
and not self.ROBOT_ERROR_DETECTED):
self.curr_velo_pub.publish(v)
rospy.sleep(0.01)
rospy.sleep(1)
self.cs.switch_controller("moveit")
self.pc.gripper.set_gripper(0.1)
self.pc.goto_saved_pose("start", velocity=0.1)
def __init__(self):
entropy_node_name = '/grasp_entropy_node'
rospy.wait_for_service(entropy_node_name + '/update_grid')
self.entropy_srv = rospy.ServiceProxy(
entropy_node_name + '/update_grid', NextViewpoint)
rospy.wait_for_service(entropy_node_name + '/reset_grid')
self.entropy_reset_srv = rospy.ServiceProxy(
entropy_node_name + '/reset_grid', EmptySrv)
rospy.wait_for_service(entropy_node_name + '/add_failure_point')
self.add_failure_point_srv = rospy.ServiceProxy(
entropy_node_name + '/add_failure_point', AddFailurePoint)
self.curr_velocity_publish_rate = 100.0 # Hz
self.curr_velo_pub = rospy.Publisher(
'/cartesian_velocity_node_controller/cartesian_velocity',
Twist,
queue_size=1)
self.max_velo = 0.1
self.curr_velo = Twist()
self.best_grasp = Pose()
self.viewpoints = 0
self.grasp_width = 0.10
self._in_velo_loop = False
self.update_rate = 10.0 # Hz
update_topic_name = '~/update'
self.update_pub = rospy.Publisher(update_topic_name,
Empty,
queue_size=1)
rospy.Subscriber(update_topic_name,
Empty,
self.__update_callback,
queue_size=1)
self.cs = ControlSwitcher({
'moveit':
'position_joint_trajectory_controller',
'velocity':
'cartesian_velocity_node_controller'
})
self.cs.switch_controller('moveit')
self.pc = PandaCommander(group_name='panda_arm')
self.robot_state = None
self.ROBOT_ERROR_DETECTED = False
self.BAD_UPDATE = False
rospy.Subscriber('/franka_state_controller/franka_states',
FrankaState,
self.__robot_state_callback,
queue_size=1)
# Centre and above the bin
self.pregrasp_pose = [
(rospy.get_param('/grasp_entropy_node/histogram/bounds/x2') +
rospy.get_param('/grasp_entropy_node/histogram/bounds/x1')) / 2,
(rospy.get_param('/grasp_entropy_node/histogram/bounds/y2') +
rospy.get_param('/grasp_entropy_node/histogram/bounds/y1')) / 2 +
0.08,
rospy.get_param('/grasp_entropy_node/height/z1') + 0.066,
2**0.5 / 2, -2**0.5 / 2, 0, 0
]
self.last_weight = 0
self.__weight_increase_check()
self.experiment = Experiment()