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)