Frankapy

FrankaArm

class frankapy.FrankaArm(rosnode_name='franka_arm_client', ros_log_level=2, robot_num=1, with_gripper=True, old_gripper=False, offline=False, init_node=True)

Bases: object

__init__(rosnode_name='franka_arm_client', ros_log_level=2, robot_num=1, with_gripper=True, old_gripper=False, offline=False, init_node=True)

Initialize a FrankaArm.

Parameters:

• rosnode_name (str) – A name for the FrankaArm ROS Node.
• ros_log_level (rospy.log_level of int) – Passed to rospy.init_node.
• robot_num (int) – Number assigned to the current robot.
• with_gripper (bool) – Flag for whether the Franka gripper is attached.
• old_gripper (bool) – Flag for whether to run the franka_ros gripper or the old_gripper implementation.
• offline (bool) – Flag for whether to run the robot in the real world.

wait_for_franka_interface(timeout=None)

Blocks execution until franka-interface gives ready signal.

Parameters:timeout (float) – Timeout in seconds to wait for franka-interface.

wait_for_skill()

Blocks execution until skill is done.

wait_for_gripper()

Blocks execution until gripper is done.

is_skill_done(ignore_errors=True)

Checks whether skill is done.

Parameters:ignore_errors (bool) – Flag of whether to ignore errors. Returns:Flag of whether the skill is done. Return type:bool

stop_skill()

Stops the current skill.

goto_pose(tool_pose, duration=3, use_impedance=True, dynamic=False, buffer_time=0.2, force_thresholds=None, torque_thresholds=None, cartesian_impedances=None, joint_impedances=None, block=True, ignore_errors=True, ignore_virtual_walls=False, skill_desc='GoToPose')

Commands Arm to the given pose via min jerk interpolation

Parameters:

• tool_pose (autolab_core.RigidTransform) – End-effector pose in tool frame
• duration (float) – How much time this robot motion should take.
• use_impedance (bool) – Function uses our impedance controller by default. If False, uses the Franka cartesian controller.
• dynamic (bool) – Flag that states whether the skill is dynamic. If True, it will use our joint impedance controller and sensor values.
• buffer_time (float) – How much extra time the termination handler will wait before stopping the skill after duration has passed.
• force_thresholds (list) – List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact.
• torque_thresholds (list) – List of 7 floats corresponding to torque limits on each joint. Default is None. If None then will not stop on contact.
• cartesian_impedances (list) – List of 6 floats corresponding to impedances on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will use default impedances.
• joint_impedances (list) – List of 7 floats corresponding to impedances on each joint. This is used when use_impedance is False. Default is None. If None then will use default impedances.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• ignore_virtual_walls (bool) – Function checks for collisions with virtual walls by default. If False, the robot no longer checks, which may be dangerous.
• skill_desc (str) – Skill description to use for logging on the Control PC.

Raises:

ValueError: If tool_pose does not have from_frame=franka_tool and – to_frame=world or tool_pose is outside the virtual walls.

goto_pose_delta(delta_tool_pose, duration=3, use_impedance=True, buffer_time=0.2, force_thresholds=None, torque_thresholds=None, cartesian_impedances=None, joint_impedances=None, block=True, ignore_errors=True, ignore_virtual_walls=False, skill_desc='GoToPoseDelta')

Commands Arm to the given delta pose via min jerk interpolation

Parameters:

• delta_tool_pose (autolab_core.RigidTransform) – Delta pose in tool frame
• duration (float) – How much time this robot motion should take.
• use_impedance (bool) – Function uses our impedance controller by default. If False, uses the Franka cartesian controller.
• buffer_time (float) – How much extra time the termination handler will wait before stopping the skill after duration has passed.
• force_thresholds (list) – List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact.
• torque_thresholds (list) – List of 7 floats corresponding to torque limits on each joint. Default is None. If None then will not stop on contact.
• cartesian_impedances (list) – List of 6 floats corresponding to impedances on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will use default impedances.
• joint_impedances (list) – List of 7 floats corresponding to impedances on each joint. This is used when use_impedance is False. Default is None. If None then will use default impedances.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• ignore_virtual_walls (bool) – Function checks for collisions with virtual walls by default. If False, the robot no longer checks, which may be dangerous.
• skill_desc (str) – Skill description to use for logging on the Control PC.

Raises:

ValueError – If delta_pose does not have from_frame=franka_tool and to_frame=franka_tool or from_frame=world and to_frame=world.

goto_joints(joints, duration=5, use_impedance=False, dynamic=False, buffer_time=0.2, force_thresholds=None, torque_thresholds=None, cartesian_impedances=None, joint_impedances=None, k_gains=None, d_gains=None, block=True, ignore_errors=True, ignore_virtual_walls=False, skill_desc='GoToJoints')

Commands Arm to the given joint configuration

Parameters:

• joints (list) – A list of 7 numbers that correspond to joint angles in radians.
• duration (float) – How much time this robot motion should take.
• use_impedance (bool) – Function uses the Franka joint impedance controller by default. If True, uses our joint impedance controller.
• dynamic (bool) – Flag that states whether the skill is dynamic. If True, it will use our joint impedance controller and sensor values.
• buffer_time (float) – How much extra time the termination handler will wait before stopping the skill after duration has passed.
• force_thresholds (list) – List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact.
• torque_thresholds (list) – List of 7 floats corresponding to torque limits on each joint. Default is None. If None then will not stop on contact.
• cartesian_impedances (list) – List of 6 floats corresponding to impedances on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will use default impedances.
• joint_impedances (list) – List of 7 floats corresponding to impedances on each joint. This is used when use_impedance is False. Default is None. If None then will use default impedances.
• k_gains (list) – List of 7 floats corresponding to the k_gains on each joint for our impedance controller. This is used when use_impedance is True. Default is None. If None then will use default k_gains.
• d_gains (list) – List of 7 floats corresponding to the d_gains on each joint for our impedance controller. This is used when use_impedance is True. Default is None. If None then will use default d_gains.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• ignore_virtual_walls (bool) – Function checks for collisions with virtual walls by default. If False, the robot no longer checks, which may be dangerous.
• skill_desc (str) – Skill description to use for logging on the Control PC.

Raises:

ValueError – If is_joints_reachable(joints) returns False

execute_cartesian_velocities(cartesian_velocities, cartesian_accelerations, duration=5, dynamic=False, buffer_time=0.2, force_thresholds=None, torque_thresholds=None, cartesian_impedances=None, joint_impedances=None, block=True, ignore_errors=True, ignore_virtual_walls=False, skill_desc='ExecutecartesianVelocities')

Commands Arm to execute cartesian velocities

Parameters:

• cartesian_velocities (list) – A list of 6 numbers that correspond to cartesian velocities in m/s and rad/s.
• cartesian_accelerations (list) – A list of 6 numbers that correspond to cartesian accelerations in m/s^2 and rad/s^2.
• duration (float) – How much time this robot motion should take.
• dynamic (bool) – Flag that states whether the skill is dynamic. If True, it will use our cartesian impedance controller and sensor values.
• buffer_time (float) – How much extra time the termination handler will wait before stopping the skill after duration has passed.
• force_thresholds (list) – List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact.
• torque_thresholds (list) – List of 7 floats corresponding to torque limits on each joint. Default is None. If None then will not stop on contact.
• cartesian_impedances (list) – List of 6 floats corresponding to impedances on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will use default impedances.
• joint_impedances (list) – List of 7 floats corresponding to impedances on each joint. This is used when use_impedance is False. Default is None. If None then will use default impedances.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• ignore_virtual_walls (bool) – Function checks for collisions with virtual walls by default. If False, the robot no longer checks, which may be dangerous.
• skill_desc (str) – Skill description to use for logging on the Control PC.

Raises:

ValueError – If is_cartesians_reachable(cartesians) returns False

execute_joint_velocities(joint_velocities, joint_accelerations, duration=5, dynamic=False, buffer_time=0.2, force_thresholds=None, torque_thresholds=None, cartesian_impedances=None, joint_impedances=None, block=True, ignore_errors=True, ignore_virtual_walls=False, skill_desc='ExecuteJointVelocities')

Commands Arm to execute joint velocities

Parameters:

• joint_velocities (list) – A list of 7 numbers that correspond to joint velocities in radians/second.
• joint_accelerations (list) – A list of 7 numbers that correspond to joint accelerations in radians/second^2.
• duration (float) – How much time this robot motion should take.
• dynamic (bool) – Flag that states whether the skill is dynamic. If True, it will use our joint impedance controller and sensor values.
• buffer_time (float) – How much extra time the termination handler will wait before stopping the skill after duration has passed.
• force_thresholds (list) – List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact.
• torque_thresholds (list) – List of 7 floats corresponding to torque limits on each joint. Default is None. If None then will not stop on contact.
• cartesian_impedances (list) – List of 6 floats corresponding to impedances on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will use default impedances.
• joint_impedances (list) – List of 7 floats corresponding to impedances on each joint. This is used when use_impedance is False. Default is None. If None then will use default impedances.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• ignore_virtual_walls (bool) – Function checks for collisions with virtual walls by default. If False, the robot no longer checks, which may be dangerous.
• skill_desc (str) – Skill description to use for logging on the Control PC.

Raises:

ValueError – If is_joints_reachable(joints) returns False

execute_joint_torques(joint_torques, selection=[0, 0, 0, 0, 0, 0, 0], remove_gravity=[0, 0, 0, 0, 0, 0, 0], duration=5, dynamic=False, buffer_time=0.2, force_thresholds=None, torque_thresholds=None, k_gains=None, d_gains=None, block=True, ignore_errors=True, ignore_virtual_walls=False, skill_desc='ExecuteJointTorques')

Commands Arm to execute joint torques

Parameters:

• joint_torques (list) – A list of 7 numbers that correspond to joint torques in radians/second.
• selection (list) – A list of 7 numbers that indicate whether to use the joint torques passed in or not. If 1 is passed in for a specific joint, the robot will use the joint torque for that joint.
• remove_gravity (list) – A list of 7 numbers that indicate whether to remove gravity from that joint or not. If 1 is passed in for a specific joint, the robot will subtract gravity for that joint.
• duration (float) – How much time this robot motion should take.
• dynamic (bool) – Flag that states whether the skill is dynamic. If True, it will use our joint impedance controller and sensor values.
• buffer_time (float) – How much extra time the termination handler will wait before stopping the skill after duration has passed.
• force_thresholds (list) – List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact.
• torque_thresholds (list) – List of 7 floats corresponding to torque limits on each joint. Default is None. If None then will not stop on contact.
• k_gains (list) – List of 7 floats corresponding to the k_gains on each joint for our impedance controller. Default is None. If None then will use default k_gains.
• d_gains (list) – List of 7 floats corresponding to the d_gains on each joint for our impedance controller. Default is None. If None then will use default d_gains.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• ignore_virtual_walls (bool) – Function checks for collisions with virtual walls by default. If False, the robot no longer checks, which may be dangerous.
• skill_desc (str) – Skill description to use for logging on the Control PC.

Raises:

ValueError – If is_joints_reachable(joints) returns False

execute_joint_dmp(joint_dmp_info, duration, initial_sensor_values=None, use_impedance=False, buffer_time=0.2, force_thresholds=None, torque_thresholds=None, cartesian_impedances=None, joint_impedances=None, k_gains=None, d_gains=None, block=True, ignore_errors=True, skill_desc='JointDmp')

Commands Arm to execute a given joint dmp

Parameters:

• joint_dmp_info (dict) – Contains all the parameters of a joint DMP (tau, alpha, beta, num_basis, num_sensors, mu, h, and weights)
• duration (float) – How much time this robot motion should take.
• initial_sensor_values (list) – List of initial sensor values. If None it will default to ones.
• use_impedance (bool) – Function uses the Franka joint impedance controller by default. If True, uses our joint impedance controller.
• buffer_time (float) – How much extra time the termination handler will wait before stopping the skill after duration has passed.
• force_thresholds (list) – List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact.
• torque_thresholds (list) – List of 7 floats corresponding to torque limits on each joint. Default is None. If None then will not stop on contact.
• cartesian_impedances (list) – List of 6 floats corresponding to impedances on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will use default impedances.
• joint_impedances (list) – List of 7 floats corresponding to impedances on each joint. This is used when use_impedance is False. Default is None. If None then will use default impedances.
• k_gains (list) – List of 7 floats corresponding to the k_gains on each joint for our impedance controller. This is used when use_impedance is True. Default is None. If None then will use default k_gains.
• d_gains (list) – List of 7 floats corresponding to the d_gains on each joint for our impedance controller. This is used when use_impedance is True. Default is None. If None then will use default d_gains.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• skill_desc (str) – Skill description to use for logging on control-pc.

execute_pose_dmp(pose_dmp_info, duration, use_goal_formulation=False, initial_sensor_values=None, orientation_only=False, position_only=False, ee_frame=False, use_impedance=True, buffer_time=0.2, force_thresholds=None, torque_thresholds=None, cartesian_impedances=None, joint_impedances=None, block=True, ignore_errors=True, skill_desc='PoseDmp')

Commands Arm to execute a given pose dmp

Parameters:

• pose_dmp_info (dict) – Contains all the parameters of a pose DMP (tau, alpha, beta, num_basis, num_sensors, mu, h, and weights)
• duration (float) – How much time this robot motion should take.
• use_goal_formulation (bool) – Flag that represents whether to use the explicit goal pose dmp formulation.
• initial_sensor_values (list) – List of initial sensor values. If None it will default to ones.
• orientation_only (bool) – Flag that represents if the dmp weights are to generate a dmp only for orientation.
• position_only (bool) – Flag that represents if the dmp weights are to generate a dmp only for position.
• ee_frame (bool) – Flag that indicates whether the dmp is in terms of the end-effector frame.
• use_impedance (bool) – Function uses our impedance controller by default. If False, uses the Franka cartesian controller.
• buffer_time (float) – How much extra time the termination handler will wait before stopping the skill after duration has passed.
• force_thresholds (list) – List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact.
• torque_thresholds (list) – List of 7 floats corresponding to torque limits on each joint. Default is None. If None then will not stop on contact.
• cartesian_impedances (list) – List of 6 floats corresponding to impedances on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will use default impedances.
• joint_impedances (list) – List of 7 floats corresponding to impedances on each joint. This is used when use_impedance is False. Default is None. If None then will use default impedances.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• skill_desc (str) – Skill description to use for logging on control-pc.

execute_quaternion_pose_dmp(position_dmp_info, quat_dmp_info, duration, goal_quat, goal_quat_time, use_goal_formulation=False, initial_sensor_values=None, ee_frame=False, use_impedance=True, buffer_time=0.2, force_thresholds=None, torque_thresholds=None, cartesian_impedances=None, joint_impedances=None, block=True, ignore_errors=True, skill_desc='QuaternionPoseDmp')

Commands Arm to execute a given quaternion pose dmp

Parameters:

• position_dmp_info – dict Contains all the parameters of a position DMP (tau, alpha, beta, num_basis, num_sensors, mu, h, and weights)
• quat_dmp_info – dict Contains all the parameters of a quaternion DMP (tau, alpha, beta, num_basis, num_sensors, mu, h, and weights)
• duration – float How much time this robot motion should take.
• goal_quat – list List of 4 floats that represents the quaternion goal to reach. This assumes an explicit goal formulation for the quaternion DMPs.
• goal_quat_time – float Time to reach goal for quaternion DMPs.
• use_goal_formulation – bool Flag that represents whether to use the explicit goal pose dmp formulation.
• initial_sensor_values – list List of initial sensor values. If None it will default to ones.
• ee_frame – bool Flag that indicates whether the dmp is in terms of the end-effector frame.
• use_impedance – bool Function uses our impedance controller by default. If False, uses the Franka cartesian controller.
• buffer_time – float How much extra time the termination handler will wait before stopping the skill after duration has passed.
• force_thresholds – list List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact.
• torque_thresholds – list List of 7 floats corresponding to torque limits on each joint. Default is None. If None then will not stop on contact.
• cartesian_impedances – list List of 6 floats corresponding to impedances on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will use default impedances.
• joint_impedances – list List of 7 floats corresponding to impedances on each joint. This is used when use_impedance is False. Default is None. If None then will use default impedances.
• block – bool Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors – bool Function ignores errors by default. If False, errors and some exceptions can be thrown.
• skill_desc – str Skill description to use for logging on control-pc.

apply_effector_forces_torques(run_duration, acc_duration, max_translation, max_rotation, forces=None, torques=None, buffer_time=0.2, force_thresholds=None, torque_thresholds=None, block=True, ignore_errors=True, skill_desc='ForceTorque')

Applies the given end-effector forces and torques in N and Nm

Parameters:

• run_duration (float) – How much time this robot motion should take.
• acc_duration (float) – How long to acc/de-acc to achieve desired force.
• max_translation (float) – A float in the unit of meters. Max translation before the robot deaccelerates.
• max_rotation (float) – A float in the unit of rad. Max rotation before the robot deaccelerates.
• forces (list) – Optional (defaults to None). A list of 3 numbers that correspond to end-effector forces in (xyz) directions.
• torques (list) – Optional (defaults to None). A list of 3 numbers that correspond to end-effector torques in 3 rotational axes.
• buffer_time (float) – How much extra time the termination handler will wait before stopping the skill after duration has passed.
• force_thresholds (list) – List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact.
• torque_thresholds (list) – List of 7 floats corresponding to torque limits on each joint. Default is None. If None then will not stop on contact.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• skill_desc (str) – Skill description to use for logging on control-pc.

Raises:

• ValueError if acc_duration > 0.5*run_duration, or if forces are
• too large

apply_effector_forces_along_axis(run_duration, acc_duration, max_translation, forces, buffer_time=0.2, force_thresholds=None, torque_thresholds=None, block=True, ignore_errors=True, skill_desc='ForcesAlongAxis')

Applies the given end-effector forces and torques in N and Nm

Parameters:

• run_duration (float) – How much time this robot motion should take.
• acc_duration (float) – How long to acc/de-acc to achieve desired force.
• max_translation (float) – A float in the unit of meters. Max translation before the robot deaccelerates.
• forces (list) – Optional (defaults to None). A list of 3 numbers that correspond to the magnitude of the end-effector forces and the axis.
• buffer_time (float) – How much extra time the termination handler will wait before stopping the skill after duration has passed.
• force_thresholds (list) – List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact.
• torque_thresholds (list) – List of 7 floats corresponding to torque limits on each joint. Default is None. If None then will not stop on contact.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• skill_desc (str) – Skill description to use for logging on control-pc.

Raises:

• ValueError if acc_duration > 0.5*run_duration, or if forces are
• too large

goto_gripper(width, grasp=False, speed=0.04, force=0.0, epsilon_inner=0.08, epsilon_outer=0.08, block=True, ignore_errors=True, skill_desc='GoToGripper')

Commands gripper to goto a certain width, applying up to the given

(default is max) force if needed

Parameters:

• width (float) – Desired width of the gripper in the unit of meters.
• grasp (bool) – Flag that signals whether to grasp.
• speed (float) – Gripper operation speed in meters per sec.
• force (float) – Max gripper force to apply in N. Default to None, which gives acceptable force.
• epsilon_inner (float) – Max tolerated deviation when the actual grasped width is smaller than the commanded grasp width.
• epsilon_outer (float) – Max tolerated deviation when the actual grasped width is larger than the commanded grasp width.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• skill_desc (str) – Skill description to use for logging on control-pc.

Raises:

ValueError – If width is less than 0 or greater than TODO(jacky) the maximum gripper opening

selective_guidance_mode(duration=5, use_joints=False, use_impedance=False, use_ee_frame=False, buffer_time=0.2, force_thresholds=None, torque_thresholds=None, cartesian_impedances=None, joint_impedances=None, k_gains=None, d_gains=None, block=True, ignore_errors=True, ignore_virtual_walls=False, skill_desc='SelectiveGuidance')

Commands the Arm to stay in its current position with selective impedances that allow guidance in either certain joints or in cartesian pose.

Parameters:

• duration (float) – How much time this guidance should take
• use_joints (bool) – Function uses cartesian impedance controller by default. If True, it uses joint impedance.
• use_impedance (bool) – Function uses the Franka joint impedance controller by default. If True, uses our joint impedance controller.
• use_ee_frame (bool) – Function uses the end-effector cartesian feedback controller only when use_impedance is True.
• buffer_time (float) – How much extra time the termination handler will wait before stopping the skill after duration has passed.
• force_thresholds (list) – List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact.
• torque_thresholds (list) – List of 7 floats corresponding to torque limits on each joint. Default is None. If None then will not stop on contact.
• cartesian_impedances (list) – List of 6 floats corresponding to impedances on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will use default impedances.
• joint_impedances (list) – List of 7 floats corresponding to impedances on each joint. This is used when use_impedance is False. Default is None. If None then will use default impedances.
• k_gains (list) – List of 7 floats corresponding to the k_gains on each joint for our impedance controller. This is used when use_impedance is True. Default is None. If None then will use default k_gains.
• d_gains (list) – List of 7 floats corresponding to the d_gains on each joint for our impedance controller. This is used when use_impedance is True. Default is None. If None then will use default d_gains.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• ignore_virtual_walls (bool) – Function checks for collisions with virtual walls by default. If False, the robot no longer checks, which may be dangerous.
• skill_desc (str) – Skill description to use for logging on control-pc.

open_gripper(block=True, skill_desc='OpenGripper')

Opens gripper to maximum width

Parameters:

• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• skill_desc (str) – Skill description to use for logging on control-pc.

close_gripper(grasp=True, block=True, skill_desc='CloseGripper')

Closes the gripper as much as possible

Parameters:

• grasp (bool) – Flag that signals whether to grasp.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• skill_desc (str) – Skill description to use for logging on control-pc.

home_gripper(block=True, skill_desc='HomeGripper')

Homes the gripper.

Parameters:

• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• skill_desc (str) – Skill description to use for logging on control-pc.

stop_gripper(block=True, skill_desc='StopGripper')

Stops the gripper.

Parameters:

• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• skill_desc (str) – Skill description to use for logging on control-pc.

run_guide_mode(duration=10, block=True, skill_desc='GuideMode')

Runs guide mode which allows you to move the robot freely.

Parameters:

• duration (float) – How much time this guidance should take
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• skill_desc (str) – Skill description to use for logging on control-pc.

run_dynamic_force_position(duration=3, buffer_time=0.2, force_thresholds=None, torque_thresholds=None, position_kps_cart=[600.0, 600.0, 600.0, 50.0, 50.0, 50.0], force_kps_cart=[0.1, 0.1, 0.1, 0.1, 0.1, 0.1], position_kps_joint=[600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0], force_kps_joint=[0.1, 0.1, 0.1, 0.1, 0.1, 0.1], S=[1, 1, 1, 1, 1, 1], interpolate=False, use_cartesian_gains=True, ignore_errors=True, ignore_virtual_walls=False, skill_desc='')

Commands Arm to run dynamic hybrid force position control

Parameters:

• duration (float) – How much time this robot motion should take.
• use_impedance (bool) – Function uses our impedance controller by default. If False, uses the Franka cartesian controller.
• buffer_time (float) – How much extra time the termination handler will wait before stopping the skill after duration has passed.
• force_thresholds (list) – List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact.
• torque_thresholds (list) – List of 7 floats corresponding to torque limits on each joint. Default is None. If None then will not stop on contact.
• position_kp_cart (list) – List of 6 floats corresponding to proportional gain used for position errors in cartesian space.
• force_kp_cart (list) – List of 6 floats corresponding to proportional gain used for force errors in cartesian space.
• position_kp_joint (list) – List of 7 floats corresponding to proportional gain used for position errors in joint space.
• force_kp_joint (list) – List of 6 floats corresponding to proportional gain used for force errors in joint space.
• S (list) – List of 6 numbers between 0 and 1 for the HFPC selection matrix.
• interpolate (bool) – Whether or not to perform linear interpolation in between way points.
• use_cartesian_gains (bool) – Whether to use cartesian gains or joint gains.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• ignore_virtual_walls (bool) – Function checks for collisions with virtual walls by default. If False, the robot no longer checks, which may be dangerous.
• skill_desc (str) – Skill description to use for logging on control-pc.

get_robot_state()

Returns:robot_state – Dict that contains all of the robot’s current state information. Return type:dict

get_pose(include_tool_offset=True)

Returns:pose – Current pose of the end-effector including the transform to the end of the tool. Return type:autolab_core.RigidTransform

get_joints()

Returns the current joint positions.

Returns:joint_positions – 7 floats that represent each joint’s position in radians. Return type:numpy.ndarray

get_joint_torques()

Returns the current joint torques.

Returns:joint_torques – 7 floats that represent each joint’s torque in Nm. Return type:numpy.ndarray

get_joint_velocities()

Returns the current joint velocities.

Returns:joint_velocities – 7 floats that represent each joint’s velocity in rads/s. Return type:numpy.ndarray

get_gripper_width()

Returns the current gripper width.

Returns:gripper_width – Robot gripper width in meters. Return type:float

get_gripper_is_grasped()

Returns a flag that represents if the gripper is grasping something.

WARNING: THIS FUNCTION HAS BEEN DEPRECATED WHEN WE SWITCHED TO THE FRANKA_ROS GRIPPER CONTROLLER BECAUSE IT DOES NOT PUBLISH THE IS GRASPED FLAG. YOU WILL NEED TO DETERMINE WHEN THE ROBOT IS GRASPING AN ITEM YOURSELF USING THE GRIPPER WIDTH.

Returns:is_grasped – Returns True if gripper is grasping something. False otherwise. Return type:bool

get_tool_base_pose()

Returns the tool delta pose.

Returns:tool_delta_pose – RigidTransform that represents the transform between the end-effector and the end of the tool. Return type:autolab_core.RigidTransform

get_ee_force_torque()

Returns the current end-effector’s sensed force_torque.

Returns:ee_force_torque – A numpy ndarray of 6 floats that represents the current end-effector’s sensed force_torque. Return type:numpy.ndarray

get_finger_poses()

Returns the current poses of the left and right fingers.

Returns:

• left_finger_pose (autolab_core.RigidTransform) – RigidTransform that represents the transform of the left finger. The left finger is +y from the gripper pose
• right_finger_pose (autolab_core.RigidTransform) – RigidTransform that represents the transform of the right finger. The right finger is -y from the gripper pose

set_tool_delta_pose(tool_delta_pose)

Sets the tool pose relative to the end-effector pose

Parameters:tool_delta_pose (autolab_core.RigidTransform) – RigidTransform that represents the transform between the end-effector and the end of the tool.

get_links_transforms(joints, use_rigid_transforms=False)

Computes the forward kinematics of all links and the end-effector

Parameters:

• joints (list) – A list of 7 numbers that correspond to to the joint angles in radians
• use_rigid_transforms (bool) – Optional: Defaults to False. If True, converts result to RigidTransform objects. This is slower.

Returns:

transforms – A list of 9 RigidTransforms or ndarrays in panda_link0 to panda_link7, the franka_tool_base, and franka_tool frames.

Return type:

list

get_jacobian(joints)

Computes the geometric jacobian

Parameters:joints (list) – A list of 7 numbers that correspond to the joint angles in radians. Returns:jacobian – A 6 by 7 jacobian matrix. Return type:numpy.ndarray

get_collision_boxes_poses(joints=None, use_rigid_transforms=False)

Computes the transforms of all collision boxes in world frame.

Parameters:

• joints (list) – Optional: Defaults to None A list of 7 numbers that correspond to to the joint angles in radians If None, will use current real robot joints.
• use_rigid_transforms (bool) – Optional: Defaults to False. If True, converts result to RigidTransform objects. This is slower.

Returns:

transforms – A list of RigidTransforms or ndarrays for all collision boxes in world frame.

Return type:

list

publish_joints(joints=None)

Publish the Franka joints to ROS

Parameters:joints (list) – Optional: Defaults to None A list of 7 numbers that correspond to to the joint angles in radians If None, will use current real robot joints.

publish_collision_boxes(joints=None)

Publish the Franka collsion boxes to ROS

Parameters:joints (list) – Optional: Defaults to None A list of 7 numbers that correspond to to the joint angles in radians. If None, will use current real robot joints.

check_box_collision(box, joints=None)

Checks if the given joint configuration is in collision with a box

Parameters:

• joints (list) – Optional: Defaults to None A list of 7 numbers that correspond to to the joint angles in radians If None, will use current real robot joints.
• box (list) – The position of the center of the box [x, y, z, r, p, y] and the length, width, and height [l, w, h].

Returns:

in_collision – Flag that indicates whether the robot would be in collision.

Return type:

bool

is_joints_in_collision_with_boxes(joints=None, boxes=None)

Checks if the given joint configuration is in collision with boxes or the workspace walls.

Parameters:

• joints (list) – Optional: Defaults to None A list of 7 numbers that correspond to the joint angles in radians If None, will use current real robot joints.
• boxes (list) – List of boxes where each box is a list of 9 floats that contains the position of the center of the box [x, y, z, r, p, y] and the length, width, and height [l, w, h].

Returns:

in_collision – Flag that indicates whether the robot would be in collision.

Return type:

bool

reset_joints(duration=5, block=True, ignore_errors=True, skill_desc='')

Commands Arm to go to hardcoded home joint configuration

Parameters:

• duration (float) – How much time this robot motion should take.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• skill_desc (str) – Skill description to use for logging on control-pc.

reset_pose(duration=5, block=True, ignore_errors=True, skill_desc='')

Commands Arm to go to hardcoded home pose

Parameters:

• duration (float) – How much time this robot motion should take.
• block (bool) – Function blocks by default. If False, the function becomes asynchronous and can be preempted.
• ignore_errors (bool) – Function ignores errors by default. If False, errors and some exceptions can be thrown.
• skill_desc (str) – Skill description to use for logging on control-pc.

is_joints_reachable(joints)

Checks if the given joint configuration is within joint limits.

Parameters:joints (list) – A list of 7 numbers that correspond to the joint angles in radians. Returns:joints_reachable – Flag that is True if all joints are within joint limits. Return type:bool

apply_joint_torques(torques, duration, ignore_errors=True, skill_desc='')

NOT IMPLEMENTED YET

Commands the arm to apply given joint torques for duration seconds.

Parameters:

• torques (list) – A list of 7 numbers that correspond to torques in Nm.
• duration (float) – How much time this robot motion should take.
• skill_desc (str) – Skill description to use for logging on control-pc.

set_speed(speed)

NOT IMPLEMENTED YET

Sets the current target speed parameter

Parameters:speed (float) – Current target speed parameter

get_speed(speed)

NOT IMPLEMENTED YET

Returns the current target speed parameter

Returns:speed – Current target speed parameter Return type:float