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()
#! /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())
#! /usr/bin/env python
import rospy
from franka_control_wrappers.panda_commander import PandaCommander
rospy.init_node('test_panda_commander')
PandaCommander()
print('Woo!')
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()