def __init__(self):
self._ns = 'franka_gripper/gripper_action'
self._gripper = franka_interface.GripperInterface(
gripper_joint_names=['panda_finger_joint1', 'panda_finger_joint2'])
# Action Server
self._server = actionlib.SimpleActionServer(
self._ns,
GripperCommandAction,
execute_cb=self._on_gripper_action,
auto_start=False)
self._action_name = "GripperActionServer"
self._server.start()
# Action Feedback/Result
self._fdbk = GripperCommandFeedback()
self._result = GripperCommandResult()
# Initialize Parameters
self._timeout = 5.0
def _configure_gripper(self, gripper_joint_names):
self._gripper = franka_interface.GripperInterface(
ns=self._ns, gripper_joint_names=gripper_joint_names)
if not self._gripper.exists:
self._gripper = None
return
camera_rot_to_gripper = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])
image_to_camera = np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])
# camera to gripper extrinsic
extrinsic = combined_RT(image_to_camera, np.array([0.0, 0.0, 0.0]))
extrinsic = np.matmul(offset_rotation, extrinsic)
extrinsic = np.matmul(offset_transition, extrinsic)
camera_to_gripper = np.matmul(
combined_RT(camera_rot_to_gripper, np.array([0.0, 0.0, 0.0])), extrinsic)
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
scene = moveit_commander.PlanningSceneInterface()
robot_arm = PandaArm()
robot_gripper = franka_interface.GripperInterface()
obj_server = object_server()
pose_estimator = densefusion_docker()
if __name__ == '__main__':
for i in range(10):
obj_server.attach_object("kinect_ros_0", 0.05, -0.05, 0.0)
# init robot arm state
robot_arm.move_to_neutral()
robot_arm.get_gripper().home_joints()
robot_arm.get_gripper().open()
print(robot_arm.ee_pose())
def __init__(self, synchronous_pub=False):
"""
"""
self.hand = franka_interface.GripperInterface()
self._params = RobotParams()
self._ns = self._params.get_base_namespace()
self._joint_limits = self._params.get_joint_limits()
joint_names = self._joint_limits.joint_names
if not joint_names:
rospy.logerr("Cannot detect joint names for arm on this "
"robot. Exiting Arm.init().")
return
self._joint_names = joint_names
self.name = self._params.get_robot_name()
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
self._stiffness_frame_effort = dict()
self._errors = dict()
self._collision_state = False
self._tip_states = None
self._jacobian = None
self._cartesian_contact = None
self._robot_mode = False
self._command_msg = JointCommand()
# neutral pose joint positions
self._neutral_pose_joints = self._params.get_neutral_pose()
self._frames_interface = FrankaFramesInterface()
try:
self._collision_behaviour_interface = CollisionBehaviourInterface()
except rospy.ROSException:
rospy.loginfo(
"Collision Service Not found. It will not be possible to change collision behaviour of robot!"
)
self._collision_behaviour_interface = None
self._ctrl_manager = FrankaControllerManagerInterface(
ns=self._ns, sim=self._params._in_sim)
self._speed_ratio = 0.15
queue_size = None if synchronous_pub else 1
with warnings.catch_warnings():
warnings.simplefilter("ignore")
self._joint_command_publisher = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=queue_size)
self._pub_joint_cmd_timeout = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_command_timeout',
Float64,
latch=True,
queue_size=10)
self._robot_state_subscriber = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/robot_state',
RobotState,
self._on_robot_state,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = self._ns + '/custom_franka_state_controller/joint_states'
self._joint_state_sub = rospy.Subscriber(joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
self._cartesian_state_sub = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/tip_state',
EndPointState,
self._on_endpoint_state,
queue_size=1,
tcp_nodelay=True)
# Cartesian Impedance Controller Publishers
self._cartesian_impedance_pose_publisher = rospy.Publisher(
"equilibrium_pose", PoseStamped, queue_size=10)
self._cartesian_stiffness_publisher = rospy.Publisher(
"impedance_stiffness", CartImpedanceStiffness, queue_size=10)
# Force Control Publisher
self._force_controller_publisher = rospy.Publisher("wrench_target",
Wrench,
queue_size=10)
# Torque Control Publisher
self._torque_controller_publisher = rospy.Publisher("torque_target",
TorqueCmd,
queue_size=20)
# Joint Impedance Controller Publishers
self._joint_impedance_publisher = rospy.Publisher(
"joint_impedance_position_velocity", JICmd, queue_size=20)
self._joint_stiffness_publisher = rospy.Publisher(
"joint_impedance_stiffness",
JointImpedanceStiffness,
queue_size=10)
rospy.on_shutdown(self._clean_shutdown)
err_msg = ("%s arm init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
franka_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current tip_state "
"from %s") % (self.name.capitalize(),
self._ns + 'tip_state')
franka_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current robot_state "
"from %s") % (self.name.capitalize(),
self._ns + 'robot_state')
franka_dataflow.wait_for(lambda: self._jacobian is not None,
timeout_msg=err_msg,
timeout=5.0)
self.set_joint_position_speed(self._speed_ratio)
