克隆下来必须带上正在用的视觉权重和本机 site.yaml，否则感知只能指到一台机器上的路径，别人起不来。
顶视迁臂改走厂家 MoveL，D405 框心对准后再对孔；夹爪停在 caught 时 open 不再把相同目标当成已经张开，init 会真的回零。 Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -0,0 +1,409 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""AGENT_REFERENCE_primitives_only.py
|
||||
|
||||
Comprehensive primitive-only reference script for code-generation agents.
|
||||
|
||||
Primitives demonstrated (in order):
|
||||
1. Home - move robot to the platform-defined home posture.
|
||||
2. ZeroFTSensor - zero force/torque sensors for all active single-arm groups.
|
||||
3. MoveJ - joint-space move with waypoints and joint-velocity scaling.
|
||||
4. MoveL - Cartesian linear move in WORLD frame with velocity, blending (zoneRadius).
|
||||
5. MoveL - multi-waypoint sweep in WORLD frame.
|
||||
6. Home - return to home before TCP-frame relative move.
|
||||
7. MoveL - relative move in TCP frame (TRAJ::START), orientation-only rotation.
|
||||
8. MoveJ - return to upright posture (all joints 0).
|
||||
9. Home - return to home.
|
||||
|
||||
Key patterns shown:
|
||||
- Fault clear, enable, wait operational, mode switch (essential setup).
|
||||
- exec_prim() wrapper accepts group-indexed PrimitiveArgs for per-group primitives.
|
||||
- Transition keys must be looked up per primitive from Flexiv primitive documentation:
|
||||
https://www.flexiv.com/software/primitives
|
||||
- Reading tcp_pose from each joint group to extract live orientation.
|
||||
- Converting quaternion to Euler ZYX degrees using utility.quat2eulerZYX for MoveL orientation.
|
||||
- ZeroFTSensor is executed for all selected groups with explicit completion checks.
|
||||
- MoveL with optional velocity, zoneRadius blending, and waypoints.
|
||||
- MoveJ with optional waypoints list and jntVelScale.
|
||||
- All JPos calls use 7-element arm-joint list; second (external-axis) argument is optional.
|
||||
- All Coord calls use positional args: (position, orientation_eulerZYX_deg, ref_frame_list).
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import argparse
|
||||
import time
|
||||
import spdlog # pip install spdlog
|
||||
import flexivrdk # pip install flexivrdk
|
||||
import utility
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────────────────────
|
||||
# Helpers
|
||||
# ──────────────────────────────────────────────────────────────────────────────
|
||||
def normalize_euler_deg(euler_deg):
|
||||
"""Normalize one angle or a 3-element Euler list to the robot's [-180, 180] convention."""
|
||||
|
||||
def normalize_angle(angle_deg):
|
||||
normalized = (angle_deg + 180.0) % 360.0 - 180.0
|
||||
if normalized == -180.0 and angle_deg > 0:
|
||||
return 180.0
|
||||
return normalized
|
||||
|
||||
if isinstance(euler_deg, (int, float)):
|
||||
return normalize_angle(euler_deg)
|
||||
|
||||
return [normalize_angle(angle) for angle in euler_deg]
|
||||
|
||||
|
||||
def wait_until_operational(robot, dt=1.0, timeout_s=120.0):
|
||||
"""Block until robot.operational() is True, or raise TimeoutError."""
|
||||
t0 = time.time()
|
||||
while not robot.operational():
|
||||
if time.time() - t0 > timeout_s:
|
||||
raise TimeoutError("Timed out waiting for robot to become operational")
|
||||
time.sleep(dt)
|
||||
|
||||
|
||||
def wait_primitive_transition(robot, transition_keys_by_group, dt=0.2, timeout_s=60.0):
|
||||
"""Block until primitive_states() for all groups satisfy their transition key.
|
||||
|
||||
Use the primitive's default transition key unless a custom transition condition
|
||||
is intended. Check primitive docs to confirm available primitive state keys.
|
||||
"""
|
||||
t0 = time.time()
|
||||
while True:
|
||||
if utility.primitive_state_true_for_groups(
|
||||
robot.primitive_states(),
|
||||
transition_keys_by_group,
|
||||
):
|
||||
return
|
||||
|
||||
if time.time() - t0 > timeout_s:
|
||||
raise TimeoutError(
|
||||
"Timed out waiting for primitive transition states: "
|
||||
f"{ {flexivrdk.kJointGroupNames[g]: k for g, k in transition_keys_by_group.items()} }"
|
||||
)
|
||||
time.sleep(dt)
|
||||
|
||||
|
||||
def exec_prim(robot, primitive_args_by_group, transition_keys_by_group):
|
||||
"""Execute primitive command(s) and block until all transitions are satisfied.
|
||||
|
||||
Args:
|
||||
robot: flexivrdk.Robot instance.
|
||||
primitive_args_by_group:
|
||||
Dict[JointGroup, flexivrdk.PrimitiveArgs] for per-group primitive commands.
|
||||
transition_keys_by_group:
|
||||
Dict[JointGroup, str] where each value is the transition key for that group.
|
||||
This can be the primitive's default key or another primitive state key.
|
||||
Look up primitive states from:
|
||||
https://www.flexiv.com/software/primitives
|
||||
|
||||
Note:
|
||||
This helper waits on primitive_states() only. If transition depends on
|
||||
robot states, implement a custom wait loop at call site.
|
||||
"""
|
||||
if not primitive_args_by_group:
|
||||
raise ValueError("primitive_args_by_group cannot be empty")
|
||||
|
||||
if set(primitive_args_by_group.keys()) != set(transition_keys_by_group.keys()):
|
||||
raise ValueError(
|
||||
"primitive_args_by_group and transition_keys_by_group must have identical groups"
|
||||
)
|
||||
|
||||
robot.ExecutePrimitive(primitive_args_by_group)
|
||||
wait_primitive_transition(robot, transition_keys_by_group)
|
||||
|
||||
|
||||
def prepare_robot(robot_sn, logger):
|
||||
"""Connect, clear faults, enable robot, and switch to primitive execution mode."""
|
||||
robot = flexivrdk.Robot(robot_sn)
|
||||
|
||||
if robot.fault():
|
||||
logger.warn("Fault detected on connected robot, trying to clear")
|
||||
if not robot.ClearFault():
|
||||
raise RuntimeError("Failed to clear robot fault")
|
||||
logger.info("Fault cleared")
|
||||
|
||||
logger.info("Enabling robot")
|
||||
robot.ServoOn()
|
||||
wait_until_operational(robot)
|
||||
logger.info("Robot is operational")
|
||||
|
||||
robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)
|
||||
return robot
|
||||
|
||||
|
||||
def current_euler_zyx_deg(robot, group):
|
||||
"""Return live TCP orientation of a specific joint group as Euler ZYX [x,y,z] degrees.
|
||||
|
||||
tcp_pose layout: [x, y, z, q_w, q_x, q_y, q_z] (SI units, quaternion).
|
||||
utility.quat2eulerZYX() expects [w, x, y, z] order.
|
||||
The returned Euler values are normalized to the robot's [-180, 180] convention.
|
||||
"""
|
||||
all_states = robot.states()
|
||||
if group not in all_states:
|
||||
raise ValueError(
|
||||
f"Requested joint group has no available robot state: "
|
||||
f"{flexivrdk.kJointGroupNames[group]}"
|
||||
)
|
||||
|
||||
pose = all_states[group].tcp_pose
|
||||
return utility.quat2eulerZYX([pose[3], pose[4], pose[5], pose[6]], degree=True)
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────────────────────
|
||||
# Main sequence
|
||||
# ──────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove spaces, e.g. Enlight-L-123456",
|
||||
)
|
||||
args = parser.parse_args()
|
||||
|
||||
logger = spdlog.ConsoleLogger("AgentReference")
|
||||
|
||||
try:
|
||||
robot = prepare_robot(args.robot_sn, logger)
|
||||
single_arm_groups = robot.info().single_arm_groups
|
||||
if not single_arm_groups:
|
||||
raise RuntimeError("No single-arm joint group found on the connected robot")
|
||||
|
||||
# ── 1) Move to home pose ──────────────────────────────────────────────
|
||||
logger.info("Step 1: Move to home pose")
|
||||
robot.Home()
|
||||
|
||||
# ── 2) ZeroFTSensor ───────────────────────────────────────────────────
|
||||
# Use this primitive's documented default transition condition.
|
||||
# The robot must not be in contact with anything during zeroing.
|
||||
logger.info("Step 2: ZeroFTSensor")
|
||||
logger.warn("Zeroing F/T sensors - ensure nothing contacts the robot")
|
||||
exec_prim(
|
||||
robot,
|
||||
{
|
||||
group: flexivrdk.PrimitiveArgs(
|
||||
"ZeroFTSensor",
|
||||
{
|
||||
"dataCollectTime": 0.2,
|
||||
"enableStaticCheck": False,
|
||||
"calibExtraPayload": False,
|
||||
},
|
||||
)
|
||||
for group in single_arm_groups
|
||||
},
|
||||
{group: "terminated" for group in single_arm_groups},
|
||||
)
|
||||
logger.info("F/T sensor zeroing complete")
|
||||
|
||||
# ── 3) MoveJ with waypoints and velocity scaling ──────────────────────
|
||||
# target / waypoints: flexivrdk.JPos(q_m) where q_m is 7-element degrees list.
|
||||
# jntVelScale: percentage of max joint speed [1-100].
|
||||
logger.info("Step 3: MoveJ with waypoints")
|
||||
exec_prim(
|
||||
robot,
|
||||
{
|
||||
group: flexivrdk.PrimitiveArgs(
|
||||
"MoveJ",
|
||||
{
|
||||
"target": flexivrdk.JPos([30, -45, 0, 90, 0, 40, 30]),
|
||||
"waypoints": [
|
||||
flexivrdk.JPos([10, -30, 10, 30, 10, 15, 10]),
|
||||
flexivrdk.JPos([20, -60, -10, 60, -10, 30, 20]),
|
||||
],
|
||||
"jntVelScale": 50, # 50% of max joint speed
|
||||
"zoneRadius": "Z50",
|
||||
"targetTolerLevel": 1,
|
||||
"enableRelativeMove": False,
|
||||
"jntAccMultiplier": 1,
|
||||
},
|
||||
)
|
||||
for group in single_arm_groups
|
||||
},
|
||||
{group: "reachedTarget" for group in single_arm_groups},
|
||||
)
|
||||
|
||||
# ── 4) MoveL in WORLD frame with velocity and blending ────────────────
|
||||
# target / waypoints: flexivrdk.Coord(position_m, orientation_eulerZYX_deg, ref_frame).
|
||||
# ref_frame is a 2-element list: [frame_type, frame_origin].
|
||||
# vel: TCP linear speed in m/s.
|
||||
# zoneRadius: blending zone size string, e.g. "Z50". Use "Z0" for exact stops.
|
||||
step4_primitive_args_by_group = {}
|
||||
for group in single_arm_groups:
|
||||
live_euler = current_euler_zyx_deg(robot, group)
|
||||
wp1_euler = normalize_euler_deg(
|
||||
[live_euler[0] + 20.0, live_euler[1] - 20.0, live_euler[2] + 10.0]
|
||||
)
|
||||
wp2_euler = normalize_euler_deg(
|
||||
[live_euler[0] - 30.0, live_euler[1] + 30.0, live_euler[2] - 30.0]
|
||||
)
|
||||
target_euler = normalize_euler_deg(
|
||||
[live_euler[0] + 40.0, live_euler[1] - 20.0, live_euler[2] + 20.0]
|
||||
)
|
||||
logger.info(
|
||||
f"Step 4 [{flexivrdk.kJointGroupNames[group]}] live Euler ZYX (deg): {live_euler}"
|
||||
)
|
||||
logger.info(
|
||||
f"Step 4 [{flexivrdk.kJointGroupNames[group]}] waypoint1 Euler ZYX (deg): {wp1_euler}"
|
||||
)
|
||||
logger.info(
|
||||
f"Step 4 [{flexivrdk.kJointGroupNames[group]}] waypoint2 Euler ZYX (deg): {wp2_euler}"
|
||||
)
|
||||
logger.info(
|
||||
f"Step 4 [{flexivrdk.kJointGroupNames[group]}] target Euler ZYX (deg): {target_euler}"
|
||||
)
|
||||
|
||||
step4_primitive_args_by_group[group] = flexivrdk.PrimitiveArgs(
|
||||
"MoveL",
|
||||
{
|
||||
"target": flexivrdk.Coord(
|
||||
[0.65, -0.3, 0.3], target_euler, ["WORLD", "WORLD_ORIGIN"]
|
||||
),
|
||||
"waypoints": [
|
||||
flexivrdk.Coord(
|
||||
[0.45, 0.1, 0.3], wp1_euler, ["WORLD", "WORLD_ORIGIN"]
|
||||
),
|
||||
flexivrdk.Coord(
|
||||
[0.45, -0.3, 0.3], wp2_euler, ["WORLD", "WORLD_ORIGIN"]
|
||||
),
|
||||
],
|
||||
"vel": 0.3, # m/s TCP linear speed
|
||||
"zoneRadius": "Z50", # blended corners; use "Z0" for hard stops
|
||||
"targetTolerLevel": 3,
|
||||
"acc": 1.5,
|
||||
"angVel": 150,
|
||||
"enableFixRefJntPos": False,
|
||||
"refJntPos": flexivrdk.JPos([0, -40, 0, 90, 0, 40, 0]),
|
||||
"jerk": 50,
|
||||
"configOptObj": [0, 0, 0.5],
|
||||
"enableSixAxisJntCtrl": False,
|
||||
"enableNullspaceTraj": False,
|
||||
},
|
||||
)
|
||||
|
||||
logger.info("Step 4: MoveL in WORLD frame with speed and blending")
|
||||
exec_prim(
|
||||
robot,
|
||||
step4_primitive_args_by_group,
|
||||
{group: "reachedTarget" for group in single_arm_groups},
|
||||
)
|
||||
|
||||
# ── 5) MoveL multi-point sweep, exact stops ───────────────────────────
|
||||
logger.info("Step 5: MoveL sweep with exact stops")
|
||||
for target_pos in [
|
||||
[0.5, 0.2, 0.4],
|
||||
[0.5, -0.2, 0.4],
|
||||
[0.5, -0.2, 0.2],
|
||||
[0.5, 0.2, 0.2],
|
||||
]:
|
||||
exec_prim(
|
||||
robot,
|
||||
{
|
||||
group: flexivrdk.PrimitiveArgs(
|
||||
"MoveL",
|
||||
{
|
||||
"target": flexivrdk.Coord(
|
||||
target_pos, [180, 0, 180], ["WORLD", "WORLD_ORIGIN"]
|
||||
),
|
||||
"vel": 0.2,
|
||||
"zoneRadius": "Z0", # exact stop at each corner
|
||||
"targetTolerLevel": 3,
|
||||
"acc": 1.5,
|
||||
"angVel": 150,
|
||||
"enableFixRefJntPos": False,
|
||||
"refJntPos": flexivrdk.JPos([0, -40, 0, 90, 0, 40, 0]),
|
||||
"jerk": 50,
|
||||
"configOptObj": [0, 0, 0.5],
|
||||
"enableSixAxisJntCtrl": False,
|
||||
"enableNullspaceTraj": False,
|
||||
},
|
||||
)
|
||||
for group in single_arm_groups
|
||||
},
|
||||
{group: "reachedTarget" for group in single_arm_groups},
|
||||
)
|
||||
|
||||
# ── 6) Home before TCP-frame relative move ────────────────────────────
|
||||
logger.info("Step 6: Move to home pose")
|
||||
robot.Home()
|
||||
|
||||
# ── 7) MoveL in TCP frame (TRAJ::START) - relative orientation change ─
|
||||
# TRAJ::START means both translation and orientation targets are relative
|
||||
# to the current TCP pose at primitive start.
|
||||
# Setting position to [0, 0, 0] keeps the TCP at the same location.
|
||||
# Orientation [20, 0, 0] means a +20 deg relative rotation around X.
|
||||
logger.info("Step 7: MoveL relative orientation-only rotation in TCP frame")
|
||||
logger.info("Relative target Euler ZYX (deg): [20.0, 0.0, 0.0]")
|
||||
exec_prim(
|
||||
robot,
|
||||
{
|
||||
group: flexivrdk.PrimitiveArgs(
|
||||
"MoveL",
|
||||
{
|
||||
# Zero position delta: stay in place. Apply a 20-degree rotation around X.
|
||||
"target": flexivrdk.Coord(
|
||||
[0.0, 0.0, 0.0],
|
||||
[20.0, 0.0, 0.0],
|
||||
["TRAJ", "START"],
|
||||
),
|
||||
"vel": 0.1,
|
||||
"zoneRadius": "Z0",
|
||||
"targetTolerLevel": 3,
|
||||
"acc": 1.0,
|
||||
"angVel": 120,
|
||||
"enableFixRefJntPos": False,
|
||||
"refJntPos": flexivrdk.JPos([0, -40, 0, 90, 0, 40, 0]),
|
||||
"jerk": 50,
|
||||
"configOptObj": [0, 0, 0.5],
|
||||
"enableSixAxisJntCtrl": False,
|
||||
"enableNullspaceTraj": False,
|
||||
},
|
||||
)
|
||||
for group in single_arm_groups
|
||||
},
|
||||
{group: "reachedTarget" for group in single_arm_groups},
|
||||
)
|
||||
|
||||
# ── 8) MoveJ to upright posture ───────────────────────────────────────
|
||||
# Upright convention: all arm joints at 0 degrees.
|
||||
logger.info("Step 8: MoveJ to upright posture")
|
||||
exec_prim(
|
||||
robot,
|
||||
{
|
||||
group: flexivrdk.PrimitiveArgs(
|
||||
"MoveJ",
|
||||
{
|
||||
"target": flexivrdk.JPos([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
|
||||
"jntVelScale": 40,
|
||||
"zoneRadius": "Z50",
|
||||
"targetTolerLevel": 1,
|
||||
"enableRelativeMove": False,
|
||||
"jntAccMultiplier": 1,
|
||||
},
|
||||
)
|
||||
for group in single_arm_groups
|
||||
},
|
||||
{group: "reachedTarget" for group in single_arm_groups},
|
||||
)
|
||||
|
||||
# ── 9) Home ───────────────────────────────────────────────────────────
|
||||
logger.info("Step 9: Move to home pose")
|
||||
robot.Home()
|
||||
|
||||
logger.info("Reference sequence completed successfully")
|
||||
robot.Stop()
|
||||
|
||||
except Exception as e:
|
||||
logger.error(f"Unhandled exception: {e}")
|
||||
return 1
|
||||
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,156 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""basics1_display_robot_states.py
|
||||
|
||||
This tutorial does the very first thing: check connection with the robot server and print
|
||||
received robot states.
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import time
|
||||
import argparse
|
||||
import threading
|
||||
import spdlog # pip install spdlog
|
||||
import flexivrdk # pip install flexivrdk
|
||||
|
||||
|
||||
def print_robot_states(robot, logger, stop_event):
|
||||
"""
|
||||
Print robot states data @ 1Hz.
|
||||
|
||||
"""
|
||||
|
||||
while not stop_event.is_set():
|
||||
# Print available joint groups
|
||||
joint_groups_str = " ".join(
|
||||
[f"[{name}]" for name in robot.info().all_groups.values()]
|
||||
)
|
||||
logger.info(f"Available joint groups: {joint_groups_str}")
|
||||
|
||||
# Print all robot states in JSON format using the built-in __str__ overloading
|
||||
for group, states in robot.states().items():
|
||||
logger.info(f"[{flexivrdk.kJointGroupNames[group]}] robot states:")
|
||||
# fmt: off
|
||||
print("{")
|
||||
print(f"timestamp: [{states.timestamp[0]}, {states.timestamp[1]}]")
|
||||
print(f"q: {['%.3f' % i for i in states.q]}")
|
||||
print(f"theta: {['%.3f' % i for i in states.theta]}")
|
||||
print(f"dq: {['%.3f' % i for i in states.dq]}")
|
||||
print(f"dtheta: {['%.3f' % i for i in states.dtheta]}")
|
||||
print(f"tau: {['%.3f' % i for i in states.tau]}")
|
||||
print(f"tau_dot: {['%.3f' % i for i in states.tau_dot]}")
|
||||
print(f"tau_ext: {['%.3f' % i for i in states.tau_ext]}")
|
||||
print(f"tau_interact: {['%.3f' % i for i in states.tau_interact]}")
|
||||
print(f"temperature: {['%.3f' % i for i in states.temperature]}")
|
||||
print(f"flange_pose: {['%.3f' % i for i in states.flange_pose]}")
|
||||
print(f"tcp_pose: {['%.3f' % i for i in states.tcp_pose]}")
|
||||
print(f"tcp_twist: {['%.3f' % i for i in states.tcp_twist]}")
|
||||
print(f"tcp_wrench: {['%.3f' % i for i in states.tcp_wrench]}")
|
||||
print(f"tcp_wrench_local: {['%.3f' % i for i in states.tcp_wrench_local]}")
|
||||
print(f"raw_tcp_wrench: {['%.3f' % i for i in states.raw_tcp_wrench]}")
|
||||
print(f"raw_tcp_wrench_local: {['%.3f' % i for i in states.raw_tcp_wrench_local]}")
|
||||
print(f"raw_ft_sensor: {['%.3f' % i for i in states.raw_ft_sensor]}")
|
||||
print("}", flush=True)
|
||||
# fmt: on
|
||||
|
||||
# Print all robot actions in JSON format using the built-in __str__ overloading
|
||||
for group, actions in robot.actions().items():
|
||||
logger.info(f"[{flexivrdk.kJointGroupNames[group]}] robot actions:")
|
||||
# fmt: off
|
||||
print("{")
|
||||
print(f"timestamp: [{actions.timestamp[0]}, {actions.timestamp[1]}]")
|
||||
print(f"q_d: {['%.3f' % i for i in actions.q_d]}")
|
||||
print(f"dq_d: {['%.3f' % i for i in actions.dq_d]}")
|
||||
print(f"tau_d: {['%.3f' % i for i in actions.tau_d]}")
|
||||
print(f"tcp_pose_d: {['%.3f' % i for i in actions.tcp_pose_d]}")
|
||||
print(f"tcp_twist_d: {['%.3f' % i for i in actions.tcp_twist_d]}")
|
||||
print(f"tcp_wrench_d: {['%.3f' % i for i in actions.tcp_wrench_d]}")
|
||||
print("}", flush=True)
|
||||
# fmt: on
|
||||
|
||||
# Print digital inputs and outputs
|
||||
logger.info("Digital inputs:")
|
||||
print(robot.digital_inputs())
|
||||
logger.info("Digital outputs:")
|
||||
print(robot.digital_outputs())
|
||||
time.sleep(1)
|
||||
|
||||
|
||||
def main():
|
||||
# Create an event to signal the thread to stop
|
||||
stop_event = threading.Event()
|
||||
|
||||
# Program Setup
|
||||
# ==============================================================================================
|
||||
# Parse arguments
|
||||
argparser = argparse.ArgumentParser()
|
||||
argparser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
|
||||
)
|
||||
args = argparser.parse_args()
|
||||
|
||||
# Define alias
|
||||
logger = spdlog.ConsoleLogger("Example")
|
||||
# Print description
|
||||
logger.info(
|
||||
">>> Tutorial description <<<\nThis tutorial does the very first thing: check connection "
|
||||
"with the robot server and print received robot states.\n"
|
||||
)
|
||||
|
||||
try:
|
||||
# RDK Initialization
|
||||
# ==========================================================================================
|
||||
# Instantiate robot interface
|
||||
robot = flexivrdk.Robot(args.robot_sn)
|
||||
|
||||
# Clear fault on the connected robot if any
|
||||
if robot.fault():
|
||||
logger.warn("Fault occurred on the connected robot, trying to clear ...")
|
||||
# Try to clear the fault
|
||||
if not robot.ClearFault():
|
||||
logger.error("Fault cannot be cleared, exiting ...")
|
||||
return 1
|
||||
logger.info("Fault on the connected robot is cleared")
|
||||
|
||||
# Servo on the robot, make sure the E-stop is released
|
||||
logger.info("Servo on the robot ...")
|
||||
robot.ServoOn()
|
||||
|
||||
# Wait for the robot to become operational
|
||||
while not robot.operational():
|
||||
time.sleep(1)
|
||||
|
||||
logger.info("Robot is now operational")
|
||||
|
||||
except Exception as e:
|
||||
# Print exception error message
|
||||
logger.error(str(e))
|
||||
return 1
|
||||
|
||||
# Print States
|
||||
# =============================================================================
|
||||
# Thread for printing robot states
|
||||
print_thread = threading.Thread(
|
||||
target=print_robot_states, args=[robot, logger, stop_event]
|
||||
)
|
||||
print_thread.start()
|
||||
|
||||
# Use main thread to catch keyboard interrupt and exit thread
|
||||
try:
|
||||
while not stop_event.is_set():
|
||||
time.sleep(0.1)
|
||||
except KeyboardInterrupt:
|
||||
# Send signal to exit thread
|
||||
logger.info("Stopping print thread")
|
||||
stop_event.set()
|
||||
|
||||
# Wait for thread to exit
|
||||
print_thread.join()
|
||||
logger.info("Print thread exited")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,62 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""basics2_clear_fault.py
|
||||
|
||||
This tutorial clears minor or critical faults, if any, of the connected robot.
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import argparse
|
||||
import spdlog # pip install spdlog
|
||||
import flexivrdk # pip install flexivrdk
|
||||
|
||||
|
||||
def main():
|
||||
# Program Setup
|
||||
# ==============================================================================================
|
||||
# Parse arguments
|
||||
argparser = argparse.ArgumentParser()
|
||||
argparser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
|
||||
)
|
||||
args = argparser.parse_args()
|
||||
|
||||
# Define alias
|
||||
logger = spdlog.ConsoleLogger("Example")
|
||||
|
||||
# Print description
|
||||
logger.info(
|
||||
">>> Tutorial description <<<\nThis tutorial clears minor or critical faults, if any, of "
|
||||
"the connected robot.\n"
|
||||
)
|
||||
|
||||
try:
|
||||
# RDK Initialization
|
||||
# ==========================================================================================
|
||||
# Instantiate robot interface
|
||||
robot = flexivrdk.Robot(args.robot_sn)
|
||||
|
||||
# Fault Clearing
|
||||
# ==========================================================================================
|
||||
# Clear fault on the connected robot if any
|
||||
if robot.fault():
|
||||
logger.warn("Fault occurred on the connected robot, trying to clear ...")
|
||||
# Try to clear the fault
|
||||
if not robot.ClearFault():
|
||||
logger.error("Fault cannot be cleared, exiting ...")
|
||||
return 1
|
||||
logger.info("Fault on the connected robot is cleared")
|
||||
else:
|
||||
logger.info("No fault on the connected robot")
|
||||
|
||||
except Exception as e:
|
||||
# Print exception error message
|
||||
logger.error(str(e))
|
||||
return 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,223 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""basics3_primitive_execution.py
|
||||
|
||||
This tutorial executes several basic robot primitives (unit skills). For detailed documentation
|
||||
on all available primitives, please see [Flexiv Primitives](https://www.flexiv.com/primitives/).
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import time
|
||||
import argparse
|
||||
import spdlog # pip install spdlog
|
||||
import flexivrdk # pip install flexivrdk
|
||||
import utility
|
||||
|
||||
|
||||
def main():
|
||||
# Program Setup
|
||||
# ==============================================================================================
|
||||
# Parse arguments
|
||||
argparser = argparse.ArgumentParser()
|
||||
argparser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
|
||||
)
|
||||
args = argparser.parse_args()
|
||||
|
||||
# Define alias
|
||||
logger = spdlog.ConsoleLogger("Example")
|
||||
mode = flexivrdk.Mode
|
||||
|
||||
# Print description
|
||||
logger.info(
|
||||
">>> Tutorial description <<<\nThis tutorial executes several basic robot primitives (unit "
|
||||
"skills). For detailed documentation on all available primitives, please see [Flexiv "
|
||||
"Primitives](https://www.flexiv.com/primitives/).\n"
|
||||
)
|
||||
|
||||
try:
|
||||
# RDK Initialization
|
||||
# ==========================================================================================
|
||||
# Instantiate robot interface
|
||||
robot = flexivrdk.Robot(args.robot_sn)
|
||||
|
||||
# Clear fault on the connected robot if any
|
||||
if robot.fault():
|
||||
logger.warn("Fault occurred on the connected robot, trying to clear ...")
|
||||
# Try to clear the fault
|
||||
if not robot.ClearFault():
|
||||
logger.error("Fault cannot be cleared, exiting ...")
|
||||
return 1
|
||||
logger.info("Fault on the connected robot is cleared")
|
||||
|
||||
# Servo on the robot, make sure the E-stop is released
|
||||
logger.info("Servo on the robot ...")
|
||||
robot.ServoOn()
|
||||
|
||||
# Wait for the robot to become operational
|
||||
while not robot.operational():
|
||||
time.sleep(1)
|
||||
|
||||
logger.info("Robot is now operational")
|
||||
|
||||
# Execute Primitives
|
||||
# ==========================================================================================
|
||||
# Primitives can only be executed on single-arm joint groups
|
||||
single_arm_groups = robot.info().single_arm_groups
|
||||
if not single_arm_groups:
|
||||
raise RuntimeError("No single-arm joint group found on the connected robot")
|
||||
|
||||
# (1) Move robot to home pose
|
||||
# ------------------------------------------------------------------------------------------
|
||||
logger.info("Moving to home pose")
|
||||
robot.Home()
|
||||
|
||||
# Switch to primitive execution mode
|
||||
robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION)
|
||||
|
||||
# (2) Move robot joints to target positions
|
||||
# ------------------------------------------------------------------------------------------
|
||||
# Required parameters:
|
||||
# target: final joint positions, unit: degrees
|
||||
# {arm joint positions}, {external axis joint positions, optional}
|
||||
# Optional parameters:
|
||||
# waypoints: waypoints to pass before reaching the target
|
||||
# (same format as above, but can be more than one)
|
||||
# vel: TCP linear velocity, unit: m/s
|
||||
logger.info("Executing primitive: MoveJ")
|
||||
|
||||
# Send command to robot
|
||||
robot.ExecutePrimitive(
|
||||
{
|
||||
group: flexivrdk.PrimitiveArgs(
|
||||
"MoveJ",
|
||||
{
|
||||
"target": flexivrdk.JPos(
|
||||
[30, -45, 0, 90, 0, 40, 30], [-50, 30, 0, 0, 0, 0]
|
||||
),
|
||||
"waypoints": [
|
||||
flexivrdk.JPos(
|
||||
[10, -30, 10, 30, 10, 15, 10], [-15, 10, 0, 0, 0, 0]
|
||||
),
|
||||
flexivrdk.JPos(
|
||||
[20, -60, -10, 60, -10, 30, 20], [-30, 20, 0, 0, 0, 0]
|
||||
),
|
||||
],
|
||||
},
|
||||
)
|
||||
for group in single_arm_groups
|
||||
}
|
||||
)
|
||||
# Most primitives won't exit by themselves and require users to explicitly trigger
|
||||
# transitions based on specific primitive states. Here we check if the primitive state
|
||||
# [reachedTarget] becomes true and trigger the transition manually by sending a new
|
||||
# primitive command.
|
||||
while True:
|
||||
primitive_states = robot.primitive_states()
|
||||
if utility.primitive_state_true_for_groups(
|
||||
primitive_states, "reachedTarget"
|
||||
):
|
||||
break
|
||||
# Print current primitive states
|
||||
logger.info("Current primitive states:")
|
||||
for group, pt_states in primitive_states.items():
|
||||
print(f"{flexivrdk.kJointGroupNames[group]}:")
|
||||
print(f"primitiveName: {pt_states.pt_name}")
|
||||
for name, value in pt_states.names_and_values.items():
|
||||
print(f"{name}: {value}")
|
||||
time.sleep(1)
|
||||
|
||||
# (3) Move robot TCP to a target pose in world (base) frame
|
||||
# ------------------------------------------------------------------------------------------
|
||||
# Required parameters:
|
||||
# target: final TCP pose, unit: m and degrees
|
||||
# {pos_x, pos_y, pos_z}, {rot_x, rot_y, rot_z}, {ref_frame, ref_point}
|
||||
# Optional parameters:
|
||||
# waypoints: waypoints to pass before reaching the target
|
||||
# (same format as above, but can be more than one)
|
||||
# vel: TCP linear velocity, unit: m/s
|
||||
# NOTE: The rotations use Euler ZYX convention, rot_x means Euler ZYX angle around X axis
|
||||
logger.info("Executing primitive: MoveL")
|
||||
|
||||
# Send command to robot
|
||||
robot.ExecutePrimitive(
|
||||
{
|
||||
group: flexivrdk.PrimitiveArgs(
|
||||
"MoveL",
|
||||
{
|
||||
"target": flexivrdk.Coord(
|
||||
[0.3, -0.1, 0.2], [160, 20, 180], ["WORLD", "WORLD_ORIGIN"]
|
||||
),
|
||||
"waypoints": [
|
||||
flexivrdk.Coord(
|
||||
[0.1, 0.1, 0.1],
|
||||
[-160, -20, 180],
|
||||
["WORLD", "WORLD_ORIGIN"],
|
||||
),
|
||||
flexivrdk.Coord(
|
||||
[0.3, 0.2, 0.1],
|
||||
[180, 0, 180],
|
||||
["WORLD", "WORLD_ORIGIN"],
|
||||
),
|
||||
],
|
||||
"vel": 0.6,
|
||||
"zoneRadius": "Z50",
|
||||
},
|
||||
)
|
||||
for group in single_arm_groups
|
||||
}
|
||||
)
|
||||
# Wait for reached target
|
||||
while not utility.primitive_state_true_for_groups(
|
||||
robot.primitive_states(), "reachedTarget"
|
||||
):
|
||||
time.sleep(1)
|
||||
|
||||
# (4) Another MoveL that uses TCP frame
|
||||
# ------------------------------------------------------------------------------------------
|
||||
# In this example the reference frame is changed from WORLD::WORLD_ORIGIN to TRAJ::START,
|
||||
# which represents the current TCP frame
|
||||
logger.info("Executing primitive: MoveL")
|
||||
|
||||
# Example to convert target quaternion [w,x,y,z] to Euler ZYX using scipy package's 'xyz'
|
||||
# extrinsic rotation
|
||||
target_quat = [0.9185587, 0.1767767, 0.3061862, 0.1767767]
|
||||
# ZYX = [30, 30, 30] degrees
|
||||
eulerZYX_deg = utility.quat2eulerZYX(target_quat, degree=True)
|
||||
|
||||
# Send command to robot. This motion will hold current TCP position and only do rotation
|
||||
robot.ExecutePrimitive(
|
||||
{
|
||||
group: flexivrdk.PrimitiveArgs(
|
||||
"MoveL",
|
||||
{
|
||||
"target": flexivrdk.Coord(
|
||||
[0.0, 0.0, 0.0], eulerZYX_deg, ["TRAJ", "START"]
|
||||
),
|
||||
"vel": 0.2,
|
||||
},
|
||||
)
|
||||
for group in single_arm_groups
|
||||
}
|
||||
)
|
||||
|
||||
# Wait for reached target
|
||||
while not utility.primitive_state_true_for_groups(
|
||||
robot.primitive_states(), "reachedTarget"
|
||||
):
|
||||
time.sleep(1)
|
||||
|
||||
# All done, stop robot and put into IDLE mode
|
||||
robot.Stop()
|
||||
|
||||
except Exception as e:
|
||||
# Print exception error message
|
||||
logger.error(str(e))
|
||||
return 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,147 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""basics4_plan_execution.py
|
||||
|
||||
This tutorial executes a plan selected by the user from a list of available plans. A plan is a
|
||||
pre-written script to execute a series of robot primitives with pre-defined transition conditions
|
||||
between 2 adjacent primitives. Users can use Flexiv Elements to compose their own plan and assign
|
||||
to the robot, which will appear in the plan list.
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import time
|
||||
import argparse
|
||||
import spdlog # pip install spdlog
|
||||
import flexivrdk # pip install flexivrdk
|
||||
|
||||
|
||||
def main():
|
||||
# Program Setup
|
||||
# ==============================================================================================
|
||||
# Parse arguments
|
||||
argparser = argparse.ArgumentParser()
|
||||
argparser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
|
||||
)
|
||||
args = argparser.parse_args()
|
||||
|
||||
# Define alias
|
||||
logger = spdlog.ConsoleLogger("Example")
|
||||
mode = flexivrdk.Mode
|
||||
|
||||
# Print description
|
||||
logger.info(
|
||||
">>> Tutorial description <<<\nThis tutorial executes a plan selected by the user from a "
|
||||
"list of available plans. A plan is a pre-written script to execute a series of robot "
|
||||
"primitives with pre-defined transition conditions between 2 adjacent primitives. Users "
|
||||
"can use Flexiv Elements to compose their own plan and assign to the robot, which "
|
||||
"will appear in the plan list.\n"
|
||||
)
|
||||
|
||||
try:
|
||||
# RDK Initialization
|
||||
# ==========================================================================================
|
||||
# Instantiate robot interface
|
||||
robot = flexivrdk.Robot(args.robot_sn)
|
||||
|
||||
# Clear fault on the connected robot if any
|
||||
if robot.fault():
|
||||
logger.warn("Fault occurred on the connected robot, trying to clear ...")
|
||||
# Try to clear the fault
|
||||
if not robot.ClearFault():
|
||||
logger.error("Fault cannot be cleared, exiting ...")
|
||||
return 1
|
||||
logger.info("Fault on the connected robot is cleared")
|
||||
|
||||
# Servo on the robot, make sure the E-stop is released
|
||||
logger.info("Servo on the robot ...")
|
||||
robot.ServoOn()
|
||||
|
||||
# Wait for the robot to become operational
|
||||
while not robot.operational():
|
||||
time.sleep(1)
|
||||
|
||||
logger.info("Robot is now operational")
|
||||
|
||||
# Execute Plans
|
||||
# ==========================================================================================
|
||||
# Switch to plan execution mode
|
||||
robot.SwitchMode(mode.NRT_PLAN_EXECUTION)
|
||||
|
||||
while True:
|
||||
# Monitor fault on the connected robot
|
||||
if robot.fault():
|
||||
raise Exception("Fault occurred on the connected robot, exiting ...")
|
||||
|
||||
# Get user input
|
||||
logger.info("Choose an action:")
|
||||
print("[1] Show available plans")
|
||||
print("[2] Execute a plan by index")
|
||||
print("[3] Execute a plan by name")
|
||||
user_input = int(input())
|
||||
|
||||
# Get and show plan list
|
||||
if user_input == 1:
|
||||
plan_list = robot.plan_list()
|
||||
for i in range(len(plan_list)):
|
||||
print(f"[{i}] {plan_list[i]}")
|
||||
print("")
|
||||
|
||||
# Execute plan by index
|
||||
elif user_input == 2:
|
||||
index = int(input("Enter plan index to execute:\n"))
|
||||
# Allow the plan to continue its execution even if the RDK program is closed or
|
||||
# the connection is lost
|
||||
robot.ExecutePlan(index, True)
|
||||
|
||||
# Print plan info while the current plan is running
|
||||
while robot.busy():
|
||||
plan_info = robot.plan_info()
|
||||
logger.info("Current plan info:")
|
||||
print(f"assigned_plan_name: {plan_info.assigned_plan_name}")
|
||||
print(f"pt_name: {plan_info.pt_name}")
|
||||
print(f"node_name: {plan_info.node_name}")
|
||||
print(f"node_path: {plan_info.node_path}")
|
||||
print(f"node_path_time_period: {plan_info.node_path_time_period}")
|
||||
print(f"node_path_number: {plan_info.node_path_number}")
|
||||
print(f"velocity_scale: {plan_info.velocity_scale}")
|
||||
print(f"waiting_for_step: {plan_info.waiting_for_step}")
|
||||
print("", flush=True)
|
||||
time.sleep(1)
|
||||
|
||||
# Execute plan by name
|
||||
elif user_input == 3:
|
||||
name = str(input("Enter plan name to execute:\n"))
|
||||
# Allow the plan to continue its execution even if the RDK program is closed or
|
||||
# the connection is lost
|
||||
robot.ExecutePlan(name, True)
|
||||
|
||||
# Print plan info while the current plan is running
|
||||
while robot.busy():
|
||||
plan_info = robot.plan_info()
|
||||
logger.info("Current plan info:")
|
||||
print(f"assigned_plan_name: {plan_info.assigned_plan_name}")
|
||||
print(f"pt_name: {plan_info.pt_name}")
|
||||
print(f"node_name: {plan_info.node_name}")
|
||||
print(f"node_path: {plan_info.node_path}")
|
||||
print(f"node_path_time_period: {plan_info.node_path_time_period}")
|
||||
print(f"node_path_number: {plan_info.node_path_number}")
|
||||
print(f"velocity_scale: {plan_info.velocity_scale}")
|
||||
print(f"waiting_for_step: {plan_info.waiting_for_step}")
|
||||
print("", flush=True)
|
||||
time.sleep(1)
|
||||
|
||||
else:
|
||||
logger.warn("Invalid input")
|
||||
|
||||
except Exception as e:
|
||||
# Print exception error message
|
||||
logger.error(str(e))
|
||||
return 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,112 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""basics5_zero_force_torque_sensors.py
|
||||
|
||||
This tutorial zeros the robot's force and torque sensors, which is a recommended (but not
|
||||
mandatory) step before any operations that require accurate force/torque measurement.
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import time
|
||||
import argparse
|
||||
import spdlog # pip install spdlog
|
||||
import flexivrdk # pip install flexivrdk
|
||||
import utility
|
||||
|
||||
|
||||
def main():
|
||||
# Program Setup
|
||||
# ==============================================================================================
|
||||
# Parse arguments
|
||||
argparser = argparse.ArgumentParser()
|
||||
argparser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
|
||||
)
|
||||
args = argparser.parse_args()
|
||||
|
||||
# Define alias
|
||||
logger = spdlog.ConsoleLogger("Example")
|
||||
mode = flexivrdk.Mode
|
||||
|
||||
# Print description
|
||||
logger.info(
|
||||
">>> Tutorial description <<<\nThis tutorial zeros the robot's force and torque sensors, "
|
||||
"which is a recommended (but not mandatory) step before any operations that require "
|
||||
"accurate force/torque measurement.\n"
|
||||
)
|
||||
|
||||
try:
|
||||
# RDK Initialization
|
||||
# ==========================================================================================
|
||||
# Instantiate robot interface
|
||||
robot = flexivrdk.Robot(args.robot_sn)
|
||||
|
||||
# Clear fault on the connected robot if any
|
||||
if robot.fault():
|
||||
logger.warn("Fault occurred on the connected robot, trying to clear ...")
|
||||
# Try to clear the fault
|
||||
if not robot.ClearFault():
|
||||
logger.error("Fault cannot be cleared, exiting ...")
|
||||
return 1
|
||||
logger.info("Fault on the connected robot is cleared")
|
||||
|
||||
# Servo on the robot, make sure the E-stop is released
|
||||
logger.info("Servo on the robot ...")
|
||||
robot.ServoOn()
|
||||
|
||||
# Wait for the robot to become operational
|
||||
while not robot.operational():
|
||||
time.sleep(1)
|
||||
|
||||
logger.info("Robot is now operational")
|
||||
|
||||
# Zero Sensors
|
||||
# ==========================================================================================
|
||||
# Get and print the current TCP force/moment readings
|
||||
for group, states in robot.states().items():
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] TCP force and moment reading in world frame BEFORE sensor zeroing: {states.tcp_wrench} N-Nm"
|
||||
)
|
||||
|
||||
# Primitives can only be executed on single-arm joint groups
|
||||
single_arm_groups = robot.info().single_arm_groups
|
||||
if not single_arm_groups:
|
||||
raise RuntimeError("No single-arm joint group found on the connected robot")
|
||||
|
||||
# Run the "ZeroFTSensor" primitive to automatically zero force and torque sensors
|
||||
robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION)
|
||||
robot.ExecutePrimitive(
|
||||
{
|
||||
group: flexivrdk.PrimitiveArgs("ZeroFTSensor", dict())
|
||||
for group in single_arm_groups
|
||||
}
|
||||
)
|
||||
|
||||
# WARNING: during the process, the robot must not contact anything, otherwise the result
|
||||
# will be inaccurate and affect following operations
|
||||
logger.warn(
|
||||
"Zeroing force/torque sensors, make sure nothing is in contact with the robot"
|
||||
)
|
||||
|
||||
# Wait for primitive to finish
|
||||
while not utility.primitive_state_true_for_groups(robot.primitive_states(), "terminated"):
|
||||
time.sleep(1)
|
||||
logger.info("Sensor zeroing complete")
|
||||
|
||||
# Get and print the current TCP force/moment readings
|
||||
for group, states in robot.states().items():
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] TCP force and moment reading in world frame AFTER sensor zeroing: {states.tcp_wrench} N-Nm"
|
||||
)
|
||||
|
||||
except Exception as e:
|
||||
# Print exception error message
|
||||
logger.error(str(e))
|
||||
return 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,230 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""basics6_gripper_control.py
|
||||
|
||||
This tutorial does position and force (if available) control of grippers supported by Flexiv.
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import time
|
||||
import sys
|
||||
import argparse
|
||||
import threading
|
||||
import spdlog # pip install spdlog
|
||||
import flexivrdk # pip install flexivrdk
|
||||
|
||||
|
||||
def print_gripper_states(gripper, logger, stop_event):
|
||||
"""
|
||||
Print gripper states data @ 1Hz.
|
||||
|
||||
"""
|
||||
while not stop_event.is_set():
|
||||
# Print all gripper states, round all float values to 2 decimals
|
||||
logger.info("Current gripper states:")
|
||||
gripper_states = gripper.states()
|
||||
for group, states in gripper_states.items():
|
||||
print(f"[{flexivrdk.kJointGroupNames[group]}]")
|
||||
print(f"width: {round(states.width, 2)}")
|
||||
print(f"force: {round(states.force, 2)}")
|
||||
print(f"is_moving: {states.is_moving}")
|
||||
print("", flush=True)
|
||||
time.sleep(1)
|
||||
|
||||
|
||||
def main():
|
||||
# Create an event to signal the thread to stop
|
||||
stop_event = threading.Event()
|
||||
|
||||
# Program Setup
|
||||
# ==============================================================================================
|
||||
# Parse arguments
|
||||
argparser = argparse.ArgumentParser()
|
||||
argparser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
|
||||
)
|
||||
argparser.add_argument(
|
||||
"gripper_device_name",
|
||||
help="Full name of the device representing the gripper, can be found in Flexiv Elements->Settings->Device",
|
||||
)
|
||||
argparser.add_argument(
|
||||
"gripper_tool_name",
|
||||
help="Full name of the tool representing the gripper, can be found in Flexiv Elements->Settings->Tool",
|
||||
)
|
||||
args = argparser.parse_args()
|
||||
|
||||
# Define alias
|
||||
logger = spdlog.ConsoleLogger("Example")
|
||||
mode = flexivrdk.Mode
|
||||
|
||||
# Print description
|
||||
logger.info(
|
||||
">>> Tutorial description <<<\nThis tutorial does position and force (if available) "
|
||||
"control of grippers supported by Flexiv.\n"
|
||||
)
|
||||
|
||||
try:
|
||||
# RDK Initialization
|
||||
# ==========================================================================================
|
||||
# Instantiate robot interface
|
||||
robot = flexivrdk.Robot(args.robot_sn)
|
||||
|
||||
# Clear fault on the connected robot if any
|
||||
if robot.fault():
|
||||
logger.warn("Fault occurred on the connected robot, trying to clear ...")
|
||||
# Try to clear the fault
|
||||
if not robot.ClearFault():
|
||||
logger.error("Fault cannot be cleared, exiting ...")
|
||||
return 1
|
||||
logger.info("Fault on the connected robot is cleared")
|
||||
|
||||
# Servo on the robot, make sure the E-stop is released
|
||||
logger.info("Servo on the robot ...")
|
||||
robot.ServoOn()
|
||||
|
||||
# Wait for the robot to become operational
|
||||
while not robot.operational():
|
||||
time.sleep(1)
|
||||
|
||||
logger.info("Robot is now operational")
|
||||
|
||||
# Gripper Control
|
||||
# ==========================================================================================
|
||||
# Instantiate gripper control interface
|
||||
gripper = flexivrdk.Gripper(robot)
|
||||
|
||||
# Instantiate tool interface. Gripper is categorized as both a device and a tool. The
|
||||
# device attribute allows a gripper to be interactively controlled by the user; whereas the
|
||||
# tool attribute tells the robot to account for its mass properties and TCP location.
|
||||
tool = flexivrdk.Tool(robot)
|
||||
|
||||
# Grippers can only be assigned to single-arm joint groups
|
||||
single_arm_groups = robot.info().single_arm_groups
|
||||
if not single_arm_groups:
|
||||
raise RuntimeError("No single-arm joint group found on the connected robot")
|
||||
|
||||
# Enable the specified gripper as a device. This is equivalent to enabling the specified
|
||||
# gripper in Flexiv Elements -> Settings -> Device
|
||||
logger.info(
|
||||
f"Enabling gripper device [{args.gripper_device_name}] for all available single-arm joint groups"
|
||||
)
|
||||
for group in single_arm_groups:
|
||||
gripper.Enable(group, args.gripper_device_name)
|
||||
|
||||
# Print parameters of the enabled gripper
|
||||
logger.info("Gripper params:")
|
||||
gripper_params = gripper.params()
|
||||
for group, params in gripper_params.items():
|
||||
print(f"[{flexivrdk.kJointGroupNames[group]}]")
|
||||
print(f"name: {params.name}")
|
||||
print(f"min_width: {round(params.min_width, 2)}")
|
||||
print(f"max_width: {round(params.max_width, 2)}")
|
||||
print(f"min_force: {round(params.min_force, 2)}")
|
||||
print(f"max_force: {round(params.max_force, 2)}")
|
||||
print(f"min_vel: {round(params.min_vel, 2)}")
|
||||
print(f"max_vel: {round(params.max_vel, 2)}")
|
||||
print("", flush=True)
|
||||
|
||||
# Switch robot tool to gripper so the gravity compensation and TCP location is updated
|
||||
logger.info(
|
||||
f"Switching robot tool to [{args.gripper_tool_name}] for all available single-arm joint groups"
|
||||
)
|
||||
for group in single_arm_groups:
|
||||
tool.Switch(group, args.gripper_tool_name)
|
||||
|
||||
# User needs to determine if this gripper requires manual initialization
|
||||
logger.info(
|
||||
"Manually trigger initialization for the gripper now? Choose Yes if it's a 48v Grav "
|
||||
"gripper"
|
||||
)
|
||||
print("[1] No, it has already initialized automatically when power on")
|
||||
print("[2] Yes, it does not initialize itself when power on")
|
||||
choice = int(input(""))
|
||||
|
||||
# Trigger manual initialization based on input
|
||||
if choice == 1:
|
||||
logger.info("Skipped manual initialization")
|
||||
elif choice == 2:
|
||||
for group in single_arm_groups:
|
||||
gripper.Init(group)
|
||||
# User determines if the manual initialization is finished
|
||||
logger.info(
|
||||
"Triggered manual initialization, press Enter when the initialization is finished to continue"
|
||||
)
|
||||
input()
|
||||
else:
|
||||
logger.error("Invalid choice")
|
||||
return 1
|
||||
|
||||
# Start a separate thread to print gripper states
|
||||
print_thread = threading.Thread(
|
||||
target=print_gripper_states, args=[gripper, logger, stop_event]
|
||||
)
|
||||
print_thread.start()
|
||||
|
||||
# Position control
|
||||
logger.info("Closing gripper")
|
||||
for group, params in gripper_params.items():
|
||||
gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force)
|
||||
time.sleep(2)
|
||||
logger.info("Opening gripper")
|
||||
for group, params in gripper_params.items():
|
||||
gripper.Move(group, params.max_width, params.max_vel, 0.25 * params.max_force)
|
||||
time.sleep(2)
|
||||
|
||||
# Stop
|
||||
logger.info("Closing gripper")
|
||||
for group, params in gripper_params.items():
|
||||
gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force)
|
||||
time.sleep(0.5)
|
||||
logger.info("Stopping gripper")
|
||||
for group in gripper_params:
|
||||
gripper.Stop(group)
|
||||
time.sleep(2)
|
||||
logger.info("Closing gripper")
|
||||
for group, params in gripper_params.items():
|
||||
gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force)
|
||||
time.sleep(2)
|
||||
logger.info("Opening gripper")
|
||||
for group, params in gripper_params.items():
|
||||
gripper.Move(group, params.max_width, params.max_vel, 0.25 * params.max_force)
|
||||
time.sleep(0.5)
|
||||
logger.info("Stopping gripper")
|
||||
for group in gripper_params:
|
||||
gripper.Stop(group)
|
||||
time.sleep(2)
|
||||
|
||||
# Force control, if available (sensed force is not zero)
|
||||
gripper_states = gripper.states()
|
||||
has_force_control = any(
|
||||
abs(states.force) > sys.float_info.epsilon
|
||||
for states in gripper_states.values()
|
||||
)
|
||||
if has_force_control:
|
||||
logger.info("Gripper running zero force control")
|
||||
for group in gripper_states:
|
||||
gripper.Grasp(group, 0)
|
||||
# Exit after 10 seconds
|
||||
time.sleep(10)
|
||||
|
||||
# Finished
|
||||
for group in gripper_states:
|
||||
gripper.Stop(group)
|
||||
|
||||
# Stop all threads
|
||||
logger.info("Stopping print thread")
|
||||
stop_event.set()
|
||||
print_thread.join()
|
||||
logger.info("Print thread exited")
|
||||
logger.info("Program finished")
|
||||
|
||||
except Exception as e:
|
||||
logger.error(str(e))
|
||||
return 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,159 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""basics7_update_robot_tool.py
|
||||
|
||||
This tutorial shows how to online update and interact with the robot tools. All changes made to
|
||||
the robot tool system will take effect immediately without needing to reboot. However, the robot
|
||||
must be put into IDLE mode when making these changes.
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import time
|
||||
import argparse
|
||||
import spdlog # pip install spdlog
|
||||
import flexivrdk # pip install flexivrdk
|
||||
|
||||
|
||||
def main():
|
||||
# Program Setup
|
||||
# ==============================================================================================
|
||||
# Parse arguments
|
||||
argparser = argparse.ArgumentParser()
|
||||
argparser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
|
||||
)
|
||||
args = argparser.parse_args()
|
||||
|
||||
# Define alias
|
||||
logger = spdlog.ConsoleLogger("Example")
|
||||
mode = flexivrdk.Mode
|
||||
|
||||
# Print description
|
||||
logger.info(
|
||||
">>> Tutorial description <<<\nThis tutorial shows how to online update and interact with "
|
||||
"the robot tools. All changes made to the robot tool system will take effect immediately "
|
||||
"without needing to reboot. However, the robot must be put into IDLE mode when making "
|
||||
"these changes.\n"
|
||||
)
|
||||
|
||||
try:
|
||||
# RDK Initialization
|
||||
# ==========================================================================================
|
||||
# Instantiate robot interface
|
||||
robot = flexivrdk.Robot(args.robot_sn)
|
||||
|
||||
# Clear fault on the connected robot if any
|
||||
if robot.fault():
|
||||
logger.warn("Fault occurred on the connected robot, trying to clear ...")
|
||||
# Try to clear the fault
|
||||
if not robot.ClearFault():
|
||||
logger.error("Fault cannot be cleared, exiting ...")
|
||||
return 1
|
||||
logger.info("Fault on the connected robot is cleared")
|
||||
|
||||
# Servo on the robot, make sure the E-stop is released
|
||||
logger.info("Servo on the robot ...")
|
||||
robot.ServoOn()
|
||||
|
||||
# Wait for the robot to become operational
|
||||
while not robot.operational():
|
||||
time.sleep(1)
|
||||
|
||||
logger.info("Robot is now operational")
|
||||
|
||||
# Update Robot Tool
|
||||
# ==========================================================================================
|
||||
# Make sure the robot is in IDLE mode
|
||||
robot.SwitchMode(mode.IDLE)
|
||||
|
||||
# Instantiate tool interface
|
||||
tool = flexivrdk.Tool(robot)
|
||||
|
||||
# Tools can only be assigned to single-arm joint groups
|
||||
single_arm_groups = robot.info().single_arm_groups
|
||||
if not single_arm_groups:
|
||||
raise RuntimeError("No single-arm joint group found on the connected robot")
|
||||
|
||||
# Get and print a list of already configured tools currently in the robot's tools pool
|
||||
logger.info("All configured tools:")
|
||||
tool_list = tool.list()
|
||||
for i in range(len(tool_list)):
|
||||
print(f"[{i}] {tool_list[i]}")
|
||||
print()
|
||||
|
||||
# Get and print the current active tool
|
||||
for group in single_arm_groups:
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] Current active tool: [{tool.name(group)}]"
|
||||
)
|
||||
|
||||
# Set name and parameters for a new tool
|
||||
new_tool_name = "ExampleTool1"
|
||||
new_tool_params = flexivrdk.ToolParams()
|
||||
new_tool_params.mass = 0.9
|
||||
new_tool_params.CoM = [0.0, 0.0, 0.057]
|
||||
new_tool_params.inertia = [2.768e-03, 3.149e-03, 5.64e-04, 0.0, 0.0, 0.0]
|
||||
new_tool_params.tcp_location = [
|
||||
0.0,
|
||||
-0.207,
|
||||
0.09,
|
||||
0.7071068,
|
||||
0.7071068,
|
||||
0.0,
|
||||
0.0,
|
||||
]
|
||||
|
||||
# If there's already a tool with the same name in the robot's tools pool, then remove it
|
||||
# first, because duplicate tool names are not allowed
|
||||
if tool.exist(new_tool_name):
|
||||
logger.warn(
|
||||
f"Tool with the same name [{new_tool_name}] already exists, removing it now"
|
||||
)
|
||||
# Switch to other tool or no tool (Flange) before removing the current tool
|
||||
for group in single_arm_groups:
|
||||
tool.Switch(group, "Flange")
|
||||
tool.Remove(new_tool_name)
|
||||
|
||||
# Add the new tool
|
||||
logger.info(f"Adding new tool [{new_tool_name}] to the robot")
|
||||
tool.Add(new_tool_name, new_tool_params)
|
||||
|
||||
# Get and print the tools list again, the new tool should appear at the end
|
||||
logger.info("All configured tools:")
|
||||
tool_list = tool.list()
|
||||
for i in range(len(tool_list)):
|
||||
print(f"[{i}] {tool_list[i]}")
|
||||
print()
|
||||
|
||||
# Switch to the newly added tool, i.e. set it as the active tool
|
||||
logger.info(f"Switching to tool [{new_tool_name}]")
|
||||
for group in single_arm_groups:
|
||||
tool.Switch(group, new_tool_name)
|
||||
|
||||
# Get and print the current active tool again, should be the new tool
|
||||
for group in single_arm_groups:
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] Current active tool: [{tool.name(group)}]"
|
||||
)
|
||||
|
||||
# Switch to other tool or no tool (Flange) before removing the current tool
|
||||
for group in single_arm_groups:
|
||||
tool.Switch(group, "Flange")
|
||||
|
||||
# Clean up by removing the new tool
|
||||
time.sleep(2)
|
||||
logger.info(f"Removing tool [{new_tool_name}]")
|
||||
tool.Remove(new_tool_name)
|
||||
|
||||
logger.info("Program finished")
|
||||
|
||||
except Exception as e:
|
||||
logger.error(str(e))
|
||||
return 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,129 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""basics8_global_variables.py
|
||||
|
||||
This tutorial shows how to get and set global variables.
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import argparse
|
||||
import spdlog # pip install spdlog
|
||||
import flexivrdk # pip install flexivrdk
|
||||
|
||||
|
||||
def main():
|
||||
# Program Setup
|
||||
# ==============================================================================================
|
||||
# Parse arguments
|
||||
argparser = argparse.ArgumentParser()
|
||||
argparser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
|
||||
)
|
||||
args = argparser.parse_args()
|
||||
|
||||
# Define alias
|
||||
logger = spdlog.ConsoleLogger("Example")
|
||||
mode = flexivrdk.Mode
|
||||
|
||||
# Print description
|
||||
logger.info(
|
||||
">>> Tutorial description <<<\nThis tutorial shows how to get and set global variables.\n"
|
||||
)
|
||||
|
||||
try:
|
||||
# RDK Initialization
|
||||
# ==========================================================================================
|
||||
# Instantiate robot interface
|
||||
robot = flexivrdk.Robot(args.robot_sn)
|
||||
|
||||
# Clear fault on the connected robot if any
|
||||
if robot.fault():
|
||||
logger.warn("Fault occurred on the connected robot, trying to clear ...")
|
||||
# Try to clear the fault
|
||||
if not robot.ClearFault():
|
||||
logger.error("Fault cannot be cleared, exiting ...")
|
||||
return 1
|
||||
logger.info("Fault on the connected robot is cleared")
|
||||
|
||||
# Servo on the robot, make sure the E-stop is released
|
||||
logger.info("Servo on the robot ...")
|
||||
robot.ServoOn()
|
||||
|
||||
# Wait for the robot to become operational
|
||||
while not robot.operational():
|
||||
time.sleep(1)
|
||||
|
||||
logger.info("Robot is now operational")
|
||||
|
||||
# Get existing global variables
|
||||
# =========================================================================================
|
||||
global_vars = robot.global_variables()
|
||||
if not global_vars:
|
||||
logger.warn("No global variables available")
|
||||
return 1
|
||||
else:
|
||||
logger.info("Existing global variables and their original values:")
|
||||
for key, value in global_vars.items():
|
||||
print(f"{key}: {value}")
|
||||
|
||||
# Set global variables
|
||||
# =========================================================================================
|
||||
# WARNING: These specified global variables need to be created first using Flexiv Elements
|
||||
logger.info("Setting new values to existing global variables")
|
||||
robot.SetGlobalVariables(
|
||||
{
|
||||
"test_bool": 1,
|
||||
"test_int": 100,
|
||||
"test_double": 100.123,
|
||||
"test_string": "Flexiv",
|
||||
"test_int_vec": [1, 2, 3],
|
||||
"test_double_vec": [1.1, 2.2, 3.3],
|
||||
"test_string_vec": ["Go", "Flexiv", "Go!"],
|
||||
"test_pose": [0.1, -0.2, 0.3, -90, -45, 120],
|
||||
"test_coord": flexivrdk.Coord(
|
||||
[0.1, -0.2, 0.3],
|
||||
[-90, -45, 120],
|
||||
["WORLD", "WORLD_ORIGIN"],
|
||||
[1, 2, 3, 4, 5, 6, 7],
|
||||
[10, 20, 0, 0, 0, 0],
|
||||
),
|
||||
"test_coord_array": [
|
||||
flexivrdk.Coord([1, 2, 3], [4, 5, 6], ["WORK", "WorkCoord0"]),
|
||||
flexivrdk.Coord(
|
||||
[10, 20, 30],
|
||||
[40, 50, 60],
|
||||
["WORLD", "WORLD_ORIGIN"],
|
||||
[1, 2, 3, 4, 5, 6, 7],
|
||||
[10, 20, 0, 0, 0, 0],
|
||||
),
|
||||
flexivrdk.Coord(
|
||||
[3, 2, 1], [180, 0, 180], ["WORLD", "WORLD_ORIGIN"]
|
||||
),
|
||||
],
|
||||
}
|
||||
)
|
||||
|
||||
# Get updated global variables
|
||||
# =========================================================================================
|
||||
global_vars = robot.global_variables()
|
||||
if not global_vars:
|
||||
logger.warn("No global variables available")
|
||||
return 1
|
||||
else:
|
||||
logger.info("Updated global variables:")
|
||||
for key, value in global_vars.items():
|
||||
print(f"{key}: {value}")
|
||||
|
||||
logger.info("Program finished")
|
||||
|
||||
except Exception as e:
|
||||
# Print exception error message
|
||||
logger.error(str(e))
|
||||
return 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+154
@@ -0,0 +1,154 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""intermediate1_non_realtime_joint_position_control.py
|
||||
|
||||
This tutorial runs non-real-time joint position control to hold or sine-sweep all robot joints.
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import time
|
||||
import math
|
||||
import argparse
|
||||
import spdlog # pip install spdlog
|
||||
import flexivrdk # pip install flexivrdk
|
||||
|
||||
|
||||
def main():
|
||||
# Program Setup
|
||||
# ==============================================================================================
|
||||
# Parse arguments
|
||||
argparser = argparse.ArgumentParser()
|
||||
# Required arguments
|
||||
argparser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
|
||||
)
|
||||
argparser.add_argument(
|
||||
"frequency", help="Command frequency, 1 to 100 [Hz]", type=int
|
||||
)
|
||||
# Optional arguments
|
||||
argparser.add_argument(
|
||||
"--hold",
|
||||
action="store_true",
|
||||
help="Robot holds current joint positions, otherwise do a sine-sweep",
|
||||
)
|
||||
args = argparser.parse_args()
|
||||
|
||||
# Check if arguments are valid
|
||||
frequency = args.frequency
|
||||
assert frequency >= 1 and frequency <= 100, "Invalid <frequency> input"
|
||||
|
||||
# Define alias
|
||||
logger = spdlog.ConsoleLogger("Example")
|
||||
mode = flexivrdk.Mode
|
||||
|
||||
# Print description
|
||||
logger.info(
|
||||
">>> Tutorial description <<<\nThis tutorial runs non-real-time joint position control to "
|
||||
"hold or sine-sweep all robot joints.\n"
|
||||
)
|
||||
|
||||
try:
|
||||
# RDK Initialization
|
||||
# ==========================================================================================
|
||||
# Instantiate robot interface
|
||||
robot = flexivrdk.Robot(args.robot_sn)
|
||||
|
||||
# Clear fault on the connected robot if any
|
||||
if robot.fault():
|
||||
logger.warn("Fault occurred on the connected robot, trying to clear ...")
|
||||
# Try to clear the fault
|
||||
if not robot.ClearFault():
|
||||
logger.error("Fault cannot be cleared, exiting ...")
|
||||
return 1
|
||||
logger.info("Fault on the connected robot is cleared")
|
||||
|
||||
# Servo on the robot, make sure the E-stop is released
|
||||
logger.info("Servo on the robot ...")
|
||||
robot.ServoOn()
|
||||
|
||||
# Wait for the robot to become operational
|
||||
while not robot.operational():
|
||||
time.sleep(1)
|
||||
|
||||
logger.info("Robot is now operational")
|
||||
|
||||
# Move robot to home pose
|
||||
logger.info("Moving to home pose")
|
||||
robot.Home()
|
||||
|
||||
# Non-real-time Joint Position Control
|
||||
# ==========================================================================================
|
||||
# Direct joint control can be executed by single-arm joint groups and the external axis
|
||||
exe_groups = robot.info().single_arm_groups
|
||||
if not exe_groups:
|
||||
raise RuntimeError("No single-arm joint group found on the connected robot")
|
||||
# The external axis joint group (if it exists) also supports direct joint control
|
||||
if flexivrdk.JointGroup.EXT_AXIS in robot.info().all_groups:
|
||||
exe_groups[flexivrdk.JointGroup.EXT_AXIS] = robot.info().all_groups[
|
||||
flexivrdk.JointGroup.EXT_AXIS
|
||||
]
|
||||
|
||||
# Switch to non-real-time joint position control mode
|
||||
robot.SwitchMode(mode.NRT_JOINT_POSITION)
|
||||
|
||||
period = 1.0 / frequency
|
||||
loop_time = 0
|
||||
logger.info(
|
||||
f"Sending command to robot at {frequency} Hz, or {period} seconds interval"
|
||||
)
|
||||
|
||||
# Use current robot joint positions as initial positions
|
||||
all_init_pos = {}
|
||||
robot_states = robot.states()
|
||||
for group in exe_groups:
|
||||
all_init_pos[group] = robot_states[group].q.copy()
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] Initial joint positions: {all_init_pos[group]}"
|
||||
)
|
||||
|
||||
# Joint sine-sweep amplitude [rad]
|
||||
SWING_AMP = 0.1
|
||||
|
||||
# TCP sine-sweep frequency [Hz]
|
||||
SWING_FREQ = 0.3
|
||||
|
||||
# Send command periodically at user-specified frequency
|
||||
while True:
|
||||
# Use sleep to control loop period
|
||||
time.sleep(period)
|
||||
|
||||
# Monitor fault on the connected robot
|
||||
if robot.fault():
|
||||
raise Exception("Fault occurred on the connected robot, exiting ...")
|
||||
|
||||
cmds = {}
|
||||
sine_offset = SWING_AMP * math.sin(2 * math.pi * SWING_FREQ * loop_time)
|
||||
for group, init_pos in all_init_pos.items():
|
||||
target_pos = init_pos.copy()
|
||||
if not args.hold:
|
||||
for i in range(len(target_pos)):
|
||||
target_pos[i] += sine_offset
|
||||
|
||||
zero_vel = [0.0] * len(init_pos)
|
||||
max_vel = [2.0] * len(init_pos)
|
||||
max_acc = [3.0] * len(init_pos)
|
||||
cmds[group] = flexivrdk.NrtJointPositionCmd(
|
||||
target_pos, zero_vel, max_vel, max_acc
|
||||
)
|
||||
|
||||
robot.SendJointPosition(cmds)
|
||||
|
||||
# Increment loop time
|
||||
loop_time += period
|
||||
|
||||
except Exception as e:
|
||||
# Print exception error message
|
||||
logger.error(str(e))
|
||||
return 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+177
@@ -0,0 +1,177 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""intermediate2_non_realtime_joint_impedance_control.py
|
||||
|
||||
This tutorial runs non-real-time joint impedance control to hold or sine-sweep all robot joints.
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import time
|
||||
import math
|
||||
import argparse
|
||||
import spdlog # pip install spdlog
|
||||
import numpy as np # pip install numpy
|
||||
import flexivrdk # pip install flexivrdk
|
||||
|
||||
|
||||
def main():
|
||||
# Program Setup
|
||||
# ==============================================================================================
|
||||
# Parse arguments
|
||||
argparser = argparse.ArgumentParser()
|
||||
# Required arguments
|
||||
argparser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
|
||||
)
|
||||
argparser.add_argument(
|
||||
"frequency", help="Command frequency, 1 to 100 [Hz]", type=int
|
||||
)
|
||||
# Optional arguments
|
||||
argparser.add_argument(
|
||||
"--hold",
|
||||
action="store_true",
|
||||
help="Robot holds current joint positions, otherwise do a sine-sweep",
|
||||
)
|
||||
args = argparser.parse_args()
|
||||
|
||||
# Check if arguments are valid
|
||||
frequency = args.frequency
|
||||
assert frequency >= 1 and frequency <= 100, "Invalid <frequency> input"
|
||||
|
||||
# Define alias
|
||||
logger = spdlog.ConsoleLogger("Example")
|
||||
mode = flexivrdk.Mode
|
||||
|
||||
# Print description
|
||||
logger.info(
|
||||
">>> Tutorial description <<<\nThis tutorial runs non-real-time joint impedance control to "
|
||||
"hold or sine-sweep all robot joints.\n"
|
||||
)
|
||||
|
||||
try:
|
||||
# RDK Initialization
|
||||
# ==========================================================================================
|
||||
# Instantiate robot interface
|
||||
robot = flexivrdk.Robot(args.robot_sn)
|
||||
|
||||
# Clear fault on the connected robot if any
|
||||
if robot.fault():
|
||||
logger.warn("Fault occurred on the connected robot, trying to clear ...")
|
||||
# Try to clear the fault
|
||||
if not robot.ClearFault():
|
||||
logger.error("Fault cannot be cleared, exiting ...")
|
||||
return 1
|
||||
logger.info("Fault on the connected robot is cleared")
|
||||
|
||||
# Servo on the robot, make sure the E-stop is released
|
||||
logger.info("Servo on the robot ...")
|
||||
robot.ServoOn()
|
||||
|
||||
# Wait for the robot to become operational
|
||||
while not robot.operational():
|
||||
time.sleep(1)
|
||||
|
||||
logger.info("Robot is now operational")
|
||||
|
||||
# Move robot to home pose
|
||||
logger.info("Moving to home pose")
|
||||
robot.Home()
|
||||
|
||||
# Non-real-time Joint Impedance Control
|
||||
# ==========================================================================================
|
||||
# Switch to non-real-time joint impedance control mode
|
||||
robot.SwitchMode(mode.NRT_JOINT_IMPEDANCE)
|
||||
|
||||
# Direct joint control can be executed by single-arm joint groups and the external axis
|
||||
single_arm_groups = robot.info().single_arm_groups
|
||||
if not single_arm_groups:
|
||||
raise RuntimeError("No single-arm joint group found on the connected robot")
|
||||
# The external axis joint group (if it exists) also supports direct joint control
|
||||
exe_groups = dict(single_arm_groups)
|
||||
if flexivrdk.JointGroup.EXT_AXIS in robot.info().all_groups:
|
||||
exe_groups[flexivrdk.JointGroup.EXT_AXIS] = robot.info().all_groups[
|
||||
flexivrdk.JointGroup.EXT_AXIS
|
||||
]
|
||||
|
||||
period = 1.0 / frequency
|
||||
loop_counter = 0
|
||||
logger.info(
|
||||
f"Sending command to robot at {frequency} Hz, or {period} seconds interval"
|
||||
)
|
||||
|
||||
# Use current robot joint positions as initial positions
|
||||
all_init_pos = {}
|
||||
robot_states = robot.states()
|
||||
for group in exe_groups:
|
||||
all_init_pos[group] = robot_states[group].q.copy()
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] Initial joint positions: {all_init_pos[group]}"
|
||||
)
|
||||
|
||||
# Joint sine-sweep amplitude [rad]
|
||||
SWING_AMP = 0.1
|
||||
|
||||
# TCP sine-sweep frequency [Hz]
|
||||
SWING_FREQ = 0.3
|
||||
|
||||
# Send command periodically at user-specified frequency
|
||||
while True:
|
||||
# Use sleep to control loop period
|
||||
time.sleep(period)
|
||||
|
||||
# Monitor fault on the connected robot
|
||||
if robot.fault():
|
||||
raise Exception("Fault occurred on the connected robot, exiting ...")
|
||||
|
||||
# Reduce stiffness to half of nominal values after 5 seconds
|
||||
if loop_counter == 5 / period:
|
||||
for group in single_arm_groups:
|
||||
new_Kq = np.multiply(robot.info().K_q_nom[group], 0.5).tolist()
|
||||
robot.SetJointImpedance(group, new_Kq)
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] Joint stiffness set to: {new_Kq}"
|
||||
)
|
||||
|
||||
# Reset stiffness to nominal values after another 5 seconds
|
||||
if loop_counter == 10 / period:
|
||||
for group in single_arm_groups:
|
||||
nominal_Kq = robot.info().K_q_nom[group]
|
||||
robot.SetJointImpedance(group, nominal_Kq)
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] Joint stiffness reset to nominal: {nominal_Kq}"
|
||||
)
|
||||
|
||||
# Send commands
|
||||
cmds = {}
|
||||
sine_offset = SWING_AMP * math.sin(
|
||||
2 * math.pi * SWING_FREQ * loop_counter * period
|
||||
)
|
||||
for group, init_pos in all_init_pos.items():
|
||||
target_pos = init_pos.copy()
|
||||
if not args.hold:
|
||||
for i in range(len(target_pos)):
|
||||
target_pos[i] += sine_offset
|
||||
|
||||
zero_vel = [0.0] * len(init_pos)
|
||||
max_vel = [2.0] * len(init_pos)
|
||||
max_acc = [3.0] * len(init_pos)
|
||||
cmds[group] = flexivrdk.NrtJointPositionCmd(
|
||||
target_pos, zero_vel, max_vel, max_acc
|
||||
)
|
||||
|
||||
robot.SendJointPosition(cmds)
|
||||
|
||||
# Increment loop counter
|
||||
loop_counter += 1
|
||||
|
||||
except Exception as e:
|
||||
# Print exception error message
|
||||
logger.error(str(e))
|
||||
return 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Vendored
+280
@@ -0,0 +1,280 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""intermediate3_non_realtime_cartesian_pure_motion_control.py
|
||||
|
||||
This tutorial runs non-real-time Cartesian-space pure motion control to hold or sine-sweep the robot
|
||||
TCP. A simple collision detection is also included.
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import time
|
||||
import math
|
||||
import argparse
|
||||
import spdlog # pip install spdlog
|
||||
import numpy as np # pip install numpy
|
||||
import flexivrdk # pip install flexivrdk
|
||||
import utility
|
||||
|
||||
# Global constants
|
||||
# ==================================================================================================
|
||||
# TCP sine-sweep amplitude [m]
|
||||
SWING_AMP = 0.1
|
||||
|
||||
# TCP sine-sweep frequency [Hz]
|
||||
SWING_FREQ = 0.3
|
||||
|
||||
# External TCP force threshold for collision detection, value is only for demo purpose [N]
|
||||
EXT_FORCE_THRESHOLD = 10.0
|
||||
|
||||
# External joint torque threshold for collision detection, value is only for demo purpose [Nm]
|
||||
EXT_TORQUE_THRESHOLD = 5.0
|
||||
|
||||
|
||||
def main():
|
||||
# Program Setup
|
||||
# ==============================================================================================
|
||||
# Parse arguments
|
||||
argparser = argparse.ArgumentParser()
|
||||
# Required arguments
|
||||
argparser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
|
||||
)
|
||||
argparser.add_argument(
|
||||
"frequency", help="Command frequency, 1 to 100 [Hz]", type=int
|
||||
)
|
||||
# Optional arguments
|
||||
argparser.add_argument(
|
||||
"--hold",
|
||||
action="store_true",
|
||||
help="Robot holds current TCP pose, otherwise do a sine-sweep",
|
||||
)
|
||||
argparser.add_argument(
|
||||
"--collision",
|
||||
action="store_true",
|
||||
help="Enable collision detection, robot will stop upon collision",
|
||||
)
|
||||
args = argparser.parse_args()
|
||||
|
||||
# Check if arguments are valid
|
||||
frequency = args.frequency
|
||||
assert frequency >= 1 and frequency <= 100, "Invalid <frequency> input"
|
||||
|
||||
# Define alias
|
||||
logger = spdlog.ConsoleLogger("Example")
|
||||
mode = flexivrdk.Mode
|
||||
|
||||
# Print description
|
||||
logger.info(
|
||||
">>> Tutorial description <<<\nThis tutorial runs non-real-time Cartesian-space pure "
|
||||
"motion control to hold or sine-sweep the robot TCP. A simple collision detection is also "
|
||||
"included.\n"
|
||||
)
|
||||
|
||||
# Print based on arguments
|
||||
if args.hold:
|
||||
logger.info("Robot holding current TCP pose")
|
||||
else:
|
||||
logger.info("Robot running TCP sine-sweep")
|
||||
|
||||
if args.collision:
|
||||
logger.info("Collision detection enabled")
|
||||
else:
|
||||
logger.info("Collision detection disabled")
|
||||
|
||||
try:
|
||||
# RDK Initialization
|
||||
# ==========================================================================================
|
||||
# Instantiate robot interface
|
||||
robot = flexivrdk.Robot(args.robot_sn)
|
||||
|
||||
# Clear fault on the connected robot if any
|
||||
if robot.fault():
|
||||
logger.warn("Fault occurred on the connected robot, trying to clear ...")
|
||||
# Try to clear the fault
|
||||
if not robot.ClearFault():
|
||||
logger.error("Fault cannot be cleared, exiting ...")
|
||||
return 1
|
||||
logger.info("Fault on the connected robot is cleared")
|
||||
|
||||
# Servo on the robot, make sure the E-stop is released
|
||||
logger.info("Servo on the robot ...")
|
||||
robot.ServoOn()
|
||||
|
||||
# Wait for the robot to become operational
|
||||
while not robot.operational():
|
||||
time.sleep(1)
|
||||
|
||||
logger.info("Robot is now operational")
|
||||
|
||||
# Move robot to home pose
|
||||
logger.info("Moving to home pose")
|
||||
robot.Home()
|
||||
|
||||
# Zero Force-torque Sensor
|
||||
# =========================================================================================
|
||||
# Direct Cartesian control can only be executed by single-arm joint groups
|
||||
single_arm_groups = robot.info().single_arm_groups
|
||||
if not single_arm_groups:
|
||||
raise RuntimeError("No single-arm joint group found on the connected robot")
|
||||
|
||||
robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION)
|
||||
# IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement
|
||||
robot.ExecutePrimitive(
|
||||
{
|
||||
group: flexivrdk.PrimitiveArgs("ZeroFTSensor", dict())
|
||||
for group in single_arm_groups
|
||||
}
|
||||
)
|
||||
|
||||
# WARNING: during the process, the robot must not contact anything, otherwise the result
|
||||
# will be inaccurate and affect following operations
|
||||
logger.warn(
|
||||
"Zeroing force/torque sensors, make sure nothing is in contact with the robot"
|
||||
)
|
||||
|
||||
# Wait for primitive to finish
|
||||
while not utility.primitive_state_true_for_groups(robot.primitive_states(), "terminated"):
|
||||
time.sleep(1)
|
||||
logger.info("Sensor zeroing complete")
|
||||
|
||||
# Configure Motion Control
|
||||
# =========================================================================================
|
||||
# The Cartesian motion force modes do pure motion control out of the box, thus nothing
|
||||
# needs to be explicitly configured
|
||||
|
||||
# NOTE: motion control always uses robot world frame, while force control can use
|
||||
# either world or TCP frame as reference frame
|
||||
|
||||
# Start Pure Motion Control
|
||||
# =========================================================================================
|
||||
# Switch to non-real-time mode for discrete motion control
|
||||
robot.SwitchMode(mode.NRT_CARTESIAN_MOTION_FORCE)
|
||||
|
||||
# Set all Cartesian axis(s) to motion control
|
||||
for group in single_arm_groups:
|
||||
robot.SetForceControlAxis(group, [False, False, False, False, False, False])
|
||||
|
||||
# Save initial poses and joint positions
|
||||
all_init_pose = {}
|
||||
all_init_q = {}
|
||||
robot_states = robot.states()
|
||||
for group in single_arm_groups:
|
||||
all_init_pose[group] = robot_states[group].tcp_pose.copy()
|
||||
all_init_q[group] = robot_states[group].q.copy()
|
||||
|
||||
# Periodic Task
|
||||
# =========================================================================================
|
||||
# Set loop period
|
||||
period = 1.0 / frequency
|
||||
loop_counter = 0
|
||||
logger.info(
|
||||
f"Sending command to robot at {frequency} Hz, or {period} seconds interval"
|
||||
)
|
||||
|
||||
# Send command periodically at user-specified frequency
|
||||
while True:
|
||||
# Use sleep to control loop period
|
||||
time.sleep(period)
|
||||
|
||||
# Monitor fault on the connected robot
|
||||
if robot.fault():
|
||||
raise Exception("Fault occurred on the connected robot, exiting ...")
|
||||
|
||||
cmds = {}
|
||||
sine_offset = SWING_AMP * math.sin(
|
||||
2 * math.pi * SWING_FREQ * loop_counter * period
|
||||
)
|
||||
for group, init_pose in all_init_pose.items():
|
||||
target_pose = init_pose.copy()
|
||||
|
||||
# Sine-sweep TCP along Y axis
|
||||
if not args.hold:
|
||||
target_pose[1] += sine_offset
|
||||
|
||||
cmds[group] = flexivrdk.NrtCartesianCmd(target_pose)
|
||||
|
||||
# Send command. Calling this method with only target pose input results
|
||||
# in pure motion control
|
||||
robot.SendCartesianMotionForce(cmds)
|
||||
|
||||
# Do the following operations in sequence for every 20 seconds
|
||||
time_elapsed = loop_counter * period
|
||||
# Online change reference joint positions at 3 seconds
|
||||
if time_elapsed % 20.0 == 3.0:
|
||||
ref_q = [0.938, -1.108, -1.254, 1.464, 1.073, 0.278, -0.658]
|
||||
for group in single_arm_groups:
|
||||
robot.SetNullSpacePosture(group, ref_q)
|
||||
logger.info(f"Reference joint positions set to: {ref_q}")
|
||||
# Online change stiffness to half of nominal at 6 seconds
|
||||
elif time_elapsed % 20.0 == 6.0:
|
||||
for group in single_arm_groups:
|
||||
new_K = np.multiply(robot.info().K_x_nom[group], 0.5).tolist()
|
||||
robot.SetCartesianImpedance(group, new_K)
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] Cartesian stiffness set to: {new_K}"
|
||||
)
|
||||
# Online change to another reference joint positions at 9 seconds
|
||||
elif time_elapsed % 20.0 == 9.0:
|
||||
ref_q = [-0.938, -1.108, 1.254, 1.464, -1.073, 0.278, 0.658]
|
||||
for group in single_arm_groups:
|
||||
robot.SetNullSpacePosture(group, ref_q)
|
||||
logger.info(f"Reference joint positions set to: {ref_q}")
|
||||
# Online reset stiffness to nominal at 12 seconds
|
||||
elif time_elapsed % 20.0 == 12.0:
|
||||
for group in single_arm_groups:
|
||||
nominal_K = robot.info().K_x_nom[group]
|
||||
robot.SetCartesianImpedance(group, nominal_K)
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] Cartesian stiffness reset to nominal: {nominal_K}"
|
||||
)
|
||||
# Online reset reference joint positions to nominal at 14 seconds
|
||||
elif time_elapsed % 20.0 == 14.0:
|
||||
for group, init_q in all_init_q.items():
|
||||
robot.SetNullSpacePosture(group, init_q)
|
||||
logger.info("Reference joint positions are reset")
|
||||
# Online enable max contact wrench regulation at 16 seconds
|
||||
elif time_elapsed % 20.0 == 16.0:
|
||||
max_wrench = [10.0, 10.0, 10.0, 2.0, 2.0, 2.0]
|
||||
for group in single_arm_groups:
|
||||
robot.SetMaxContactWrench(group, max_wrench)
|
||||
logger.info(f"Max contact wrench set to: {max_wrench}")
|
||||
# Disable max contact wrench regulation at 19 seconds
|
||||
elif time_elapsed % 20.0 == 19.0:
|
||||
for group in single_arm_groups:
|
||||
robot.SetMaxContactWrench(group, [float("inf")] * 6)
|
||||
logger.info("Max contact wrench regulation is disabled")
|
||||
|
||||
# Simple collision detection: stop robot if collision is detected at
|
||||
# end-effector
|
||||
if args.collision:
|
||||
for group, states in robot.states().items():
|
||||
collision_detected = False
|
||||
ext_force = np.array(states.tcp_wrench[:3])
|
||||
if np.linalg.norm(ext_force) > EXT_FORCE_THRESHOLD:
|
||||
collision_detected = True
|
||||
|
||||
for v in states.tau_ext:
|
||||
if abs(v) > EXT_TORQUE_THRESHOLD:
|
||||
collision_detected = True
|
||||
|
||||
if collision_detected:
|
||||
robot.Stop()
|
||||
logger.warn(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] Collision detected, stopping robot and exit program ..."
|
||||
)
|
||||
return
|
||||
|
||||
# Increment loop counter
|
||||
loop_counter += 1
|
||||
|
||||
except Exception as e:
|
||||
# Print exception error message
|
||||
logger.error(str(e))
|
||||
return 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Vendored
+300
@@ -0,0 +1,300 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""intermediate4_non_realtime_cartesian_motion_force_control.py
|
||||
|
||||
This tutorial runs non-real-time Cartesian-space unified motion-force control. The Z axis of the
|
||||
chosen reference frame will be activated for explicit force control, while the rest axes in the
|
||||
same reference frame will stay motion controlled.
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import time
|
||||
import math
|
||||
import argparse
|
||||
import spdlog # pip install spdlog
|
||||
import numpy as np # pip install numpy
|
||||
import flexivrdk # pip install flexivrdk
|
||||
import utility
|
||||
|
||||
# Global constants
|
||||
# ==================================================================================================
|
||||
# TCP sine-sweep amplitude [m]
|
||||
SWING_AMP = 0.1
|
||||
|
||||
# TCP sine-sweep frequency [Hz]
|
||||
SWING_FREQ = 0.3
|
||||
|
||||
# Pressing force to apply during the unified motion-force control [N]
|
||||
PRESSING_FORCE = 5.0
|
||||
|
||||
# Cartesian linear velocity used to search for contact [m/s]
|
||||
SEARCH_VELOCITY = 0.02
|
||||
|
||||
# Maximum distance to travel when searching for contact [m]
|
||||
SEARCH_DISTANCE = 1.0
|
||||
|
||||
# Maximum contact wrench during contact search for soft contact
|
||||
MAX_WRENCH_FOR_CONTACT_SEARCH = [10.0, 10.0, 10.0, 3.0, 3.0, 3.0]
|
||||
|
||||
|
||||
def main():
|
||||
# Program Setup
|
||||
# ==============================================================================================
|
||||
# Parse arguments
|
||||
argparser = argparse.ArgumentParser()
|
||||
# Required arguments
|
||||
argparser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
|
||||
)
|
||||
argparser.add_argument(
|
||||
"frequency", help="Command frequency, 1 to 100 [Hz]", type=int
|
||||
)
|
||||
# Optional arguments
|
||||
argparser.add_argument(
|
||||
"--TCP",
|
||||
action="store_true",
|
||||
help="Use TCP frame as reference frame for force control, otherwise use world frame",
|
||||
)
|
||||
argparser.add_argument(
|
||||
"--polish",
|
||||
action="store_true",
|
||||
help="Run a simple polish motion along XY plane in world frame, otherwise hold robot motion in non-force-control axes",
|
||||
)
|
||||
args = argparser.parse_args()
|
||||
|
||||
# Check if arguments are valid
|
||||
frequency = args.frequency
|
||||
assert frequency >= 1 and frequency <= 100, "Invalid <frequency> input"
|
||||
|
||||
# Define alias
|
||||
logger = spdlog.ConsoleLogger("Example")
|
||||
mode = flexivrdk.Mode
|
||||
|
||||
# Print description
|
||||
logger.info(
|
||||
">>> Tutorial description <<<\nThis tutorial runs non-real-time Cartesian-space unified "
|
||||
"motion-force control. The Z axis of the chosen reference frame will be activated for "
|
||||
"explicit force control, while the rest axes in the same reference frame will stay motion "
|
||||
"controlled.\n"
|
||||
)
|
||||
|
||||
# The reference frame to use, see Robot::SendCartesianMotionForce() for more details
|
||||
force_ctrl_frame = flexivrdk.CoordType.WORLD
|
||||
if args.TCP:
|
||||
logger.info("Reference frame used for force control: robot TCP frame")
|
||||
force_ctrl_frame = flexivrdk.CoordType.TCP
|
||||
else:
|
||||
logger.info("Reference frame used for force control: robot world frame")
|
||||
|
||||
# Whether to enable polish motion
|
||||
if args.polish:
|
||||
logger.info(
|
||||
"Robot will run a polish motion along XY plane in robot world frame"
|
||||
)
|
||||
else:
|
||||
logger.info("Robot will hold its motion in all non-force-controlled axes")
|
||||
|
||||
try:
|
||||
# RDK Initialization
|
||||
# ==========================================================================================
|
||||
# Instantiate robot interface
|
||||
robot = flexivrdk.Robot(args.robot_sn)
|
||||
|
||||
# Clear fault on the connected robot if any
|
||||
if robot.fault():
|
||||
logger.warn("Fault occurred on the connected robot, trying to clear ...")
|
||||
# Try to clear the fault
|
||||
if not robot.ClearFault():
|
||||
logger.error("Fault cannot be cleared, exiting ...")
|
||||
return 1
|
||||
logger.info("Fault on the connected robot is cleared")
|
||||
|
||||
# Servo on the robot, make sure the E-stop is released
|
||||
logger.info("Servo on the robot ...")
|
||||
robot.ServoOn()
|
||||
|
||||
# Wait for the robot to become operational
|
||||
while not robot.operational():
|
||||
time.sleep(1)
|
||||
|
||||
logger.info("Robot is now operational")
|
||||
|
||||
# Move robot to home pose
|
||||
logger.info("Moving to home pose")
|
||||
robot.Home()
|
||||
|
||||
# Zero Force-torque Sensor
|
||||
# =========================================================================================
|
||||
# Direct Cartesian control can only be executed by single-arm joint groups
|
||||
single_arm_groups = robot.info().single_arm_groups
|
||||
if not single_arm_groups:
|
||||
raise RuntimeError("No single-arm joint group found on the connected robot")
|
||||
|
||||
robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION)
|
||||
# IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement
|
||||
robot.ExecutePrimitive(
|
||||
{
|
||||
group: flexivrdk.PrimitiveArgs("ZeroFTSensor", dict())
|
||||
for group in single_arm_groups
|
||||
}
|
||||
)
|
||||
|
||||
# WARNING: during the process, the robot must not contact anything, otherwise the result
|
||||
# will be inaccurate and affect following operations
|
||||
logger.warn(
|
||||
"Zeroing force/torque sensors, make sure nothing is in contact with the robot"
|
||||
)
|
||||
|
||||
# Wait for primitive to finish
|
||||
while not utility.primitive_state_true_for_groups(
|
||||
robot.primitive_states(), "terminated"
|
||||
):
|
||||
time.sleep(1)
|
||||
logger.info("Sensor zeroing complete")
|
||||
|
||||
# Search for Contact
|
||||
# =========================================================================================
|
||||
# NOTE: there are several ways to do contact search, such as using primitives, or real-time
|
||||
# and non-real-time direct motion controls, etc. Here we use non-real-time direct Cartesian
|
||||
# control for example.
|
||||
logger.info("Searching for contact ...")
|
||||
|
||||
# Set initial poses to current TCP poses
|
||||
all_init_pose = {}
|
||||
for group in single_arm_groups:
|
||||
all_init_pose[group] = robot.states()[group].tcp_pose.copy()
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] Initial TCP pose [position 3x1, rotation (quaternion) 4x1]: {all_init_pose[group]}"
|
||||
)
|
||||
|
||||
# Use non-real-time mode to make the robot go to a set point with its own motion generator
|
||||
robot.SwitchMode(mode.NRT_CARTESIAN_MOTION_FORCE)
|
||||
|
||||
# Search for contact with max contact wrench set to a small value for making soft contact
|
||||
for group in single_arm_groups:
|
||||
robot.SetMaxContactWrench(group, MAX_WRENCH_FOR_CONTACT_SEARCH)
|
||||
|
||||
# Set target point along -Z direction and expect contact to happen during the travel
|
||||
cmds = {}
|
||||
for group, init_pose in all_init_pose.items():
|
||||
target_pose = init_pose.copy()
|
||||
target_pose[2] -= SEARCH_DISTANCE
|
||||
cmds[group] = flexivrdk.NrtCartesianCmd(
|
||||
target_pose, [0.0] * 6, [0.0] * 6, SEARCH_VELOCITY
|
||||
)
|
||||
|
||||
# Send target point to robot to start searching for contact and limit the velocity. Keep
|
||||
# target wrench 0 at this stage since we are not doing force control yet
|
||||
robot.SendCartesianMotionForce(cmds)
|
||||
|
||||
# Use a while loop to poll robot states and check if a contact is made
|
||||
is_contacted = False
|
||||
while not is_contacted:
|
||||
# Compute norm of sensed external force applied on robot TCP
|
||||
for group, states in robot.states().items():
|
||||
ext_force = np.array(states.tcp_wrench[:3])
|
||||
|
||||
# Contact is considered to be made if sensed TCP force exceeds the threshold
|
||||
if np.linalg.norm(ext_force) > PRESSING_FORCE:
|
||||
is_contacted = True
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] Contact detected at robot TCP"
|
||||
)
|
||||
break
|
||||
|
||||
# Check at 1ms interval
|
||||
time.sleep(0.001)
|
||||
|
||||
# Configure Force Control
|
||||
# =========================================================================================
|
||||
# Switch to non-real-time mode for discrete motion force control
|
||||
robot.SwitchMode(mode.NRT_CARTESIAN_MOTION_FORCE)
|
||||
|
||||
# Set force control reference frame based on program argument. See function doc for more
|
||||
# details
|
||||
for group in single_arm_groups:
|
||||
robot.SetForceControlFrame(group, force_ctrl_frame)
|
||||
|
||||
# Set which Cartesian axis(s) to activate for force control. See function doc for more
|
||||
# details. Here we only active Z axis
|
||||
for group in single_arm_groups:
|
||||
robot.SetForceControlAxis(group, [False, False, True, False, False, False])
|
||||
|
||||
# Uncomment the following line to enable passive force control, otherwise active force
|
||||
# control is used by default. See function doc for more details
|
||||
# for group in single_arm_groups:
|
||||
# robot.SetPassiveForceControl(group, True)
|
||||
|
||||
# NOTE: motion control always uses robot world frame, while force control can use
|
||||
# either world or TCP frame as reference frame
|
||||
|
||||
# Start Unified Motion Force Control
|
||||
# =========================================================================================
|
||||
# Disable max contact wrench regulation. Need to do this AFTER the force control in Z axis
|
||||
# is activated (i.e. motion control disabled in Z axis) and the motion force control mode
|
||||
# is entered, this way the contact force along Z axis is explicitly regulated and will not
|
||||
# spike after the max contact wrench regulation for motion control is disabled
|
||||
for group in single_arm_groups:
|
||||
robot.SetMaxContactWrench(group, [float("inf")] * 6)
|
||||
|
||||
# Update initial poses to current TCP poses
|
||||
for group in single_arm_groups:
|
||||
all_init_pose[group] = robot.states()[group].tcp_pose.copy()
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] Initial TCP pose [position 3x1, rotation (quaternion) 4x1]: {all_init_pose[group]}"
|
||||
)
|
||||
|
||||
# Periodic Task
|
||||
# =========================================================================================
|
||||
# Set loop period
|
||||
period = 1.0 / frequency
|
||||
logger.info(
|
||||
f"Sending command to robot at {frequency} Hz, or {period} seconds interval"
|
||||
)
|
||||
|
||||
# Periodic loop counter
|
||||
loop_counter = 0
|
||||
|
||||
# Send command periodically at user-specified frequency
|
||||
while True:
|
||||
# Use sleep to control loop period
|
||||
time.sleep(period)
|
||||
|
||||
# Monitor fault on the connected robot
|
||||
if robot.fault():
|
||||
raise Exception("Fault occurred on the connected robot, exiting ...")
|
||||
|
||||
# Set Fz according to reference frame to achieve a "pressing down" behavior
|
||||
Fz = 0.0
|
||||
if force_ctrl_frame == flexivrdk.CoordType.WORLD:
|
||||
Fz = PRESSING_FORCE
|
||||
elif force_ctrl_frame == flexivrdk.CoordType.TCP:
|
||||
Fz = -PRESSING_FORCE
|
||||
target_wrench = [0.0, 0.0, Fz, 0.0, 0.0, 0.0]
|
||||
|
||||
cmds = {}
|
||||
for group, init_pose in all_init_pose.items():
|
||||
target_pose = init_pose.copy()
|
||||
if args.polish:
|
||||
target_pose[1] += SWING_AMP * math.sin(
|
||||
2 * math.pi * SWING_FREQ * loop_counter * period
|
||||
)
|
||||
|
||||
cmds[group] = flexivrdk.NrtCartesianCmd(target_pose, target_wrench)
|
||||
|
||||
robot.SendCartesianMotionForce(cmds)
|
||||
|
||||
# Increment loop counter
|
||||
loop_counter += 1
|
||||
|
||||
except Exception as e:
|
||||
# Print exception error message
|
||||
logger.error(str(e))
|
||||
return 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,132 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""intermediate5_robot_dynamics.py
|
||||
|
||||
This tutorial runs the integrated dynamics engine to obtain robot Jacobian, mass matrix, and
|
||||
gravity torques. Also checks reachability of a Cartesian pose.
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import time
|
||||
import argparse
|
||||
import spdlog # pip install spdlog
|
||||
import flexivrdk # pip install flexivrdk
|
||||
|
||||
|
||||
def main():
|
||||
# Program Setup
|
||||
# ==============================================================================================
|
||||
# Parse arguments
|
||||
argparser = argparse.ArgumentParser()
|
||||
argparser.add_argument(
|
||||
"robot_sn",
|
||||
help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
|
||||
)
|
||||
args = argparser.parse_args()
|
||||
|
||||
# Define alias
|
||||
logger = spdlog.ConsoleLogger("Example")
|
||||
mode = flexivrdk.Mode
|
||||
|
||||
# Print description
|
||||
logger.info(
|
||||
">>> Tutorial description <<<\nThis tutorial runs the integrated dynamics engine to obtain "
|
||||
"robot Jacobian, mass matrix, and gravity torques. Also checks reachability of a Cartesian "
|
||||
"pose.\n"
|
||||
)
|
||||
|
||||
try:
|
||||
# RDK Initialization
|
||||
# ==========================================================================================
|
||||
# Instantiate robot interface
|
||||
robot = flexivrdk.Robot(args.robot_sn)
|
||||
|
||||
# Clear fault on the connected robot if any
|
||||
if robot.fault():
|
||||
logger.warn("Fault occurred on the connected robot, trying to clear ...")
|
||||
# Try to clear the fault
|
||||
if not robot.ClearFault():
|
||||
logger.error("Fault cannot be cleared, exiting ...")
|
||||
return 1
|
||||
logger.info("Fault on the connected robot is cleared")
|
||||
|
||||
# Servo on the robot, make sure the E-stop is released
|
||||
logger.info("Servo on the robot ...")
|
||||
robot.ServoOn()
|
||||
|
||||
# Wait for the robot to become operational
|
||||
while not robot.operational():
|
||||
time.sleep(1)
|
||||
|
||||
logger.info("Robot is now operational")
|
||||
|
||||
# Move robot to home pose
|
||||
logger.info("Moving to home pose")
|
||||
robot.Home()
|
||||
|
||||
# Robot Dynamics
|
||||
# ==========================================================================================
|
||||
# Initialize dynamics engine
|
||||
model = flexivrdk.Model(robot)
|
||||
|
||||
# Step dynamics engine 5 times
|
||||
for i in range(5):
|
||||
# Update robot model in dynamics engine
|
||||
robot_states = robot.states()
|
||||
model.Update(
|
||||
robot_states[flexivrdk.JointGroup.ALL].q,
|
||||
robot_states[flexivrdk.JointGroup.ALL].dq,
|
||||
)
|
||||
|
||||
# Compute gravity vector
|
||||
g = model.g()
|
||||
|
||||
# Compute mass matrix
|
||||
M = model.M()
|
||||
|
||||
# Compute Jacobian
|
||||
J = model.J("flange")
|
||||
|
||||
# Print result
|
||||
logger.info("g = ")
|
||||
print(g, flush=True)
|
||||
logger.info("M = ")
|
||||
print(M, flush=True)
|
||||
logger.info("J = ")
|
||||
print(J, flush=True)
|
||||
print()
|
||||
|
||||
# Check IK feasibility for a nearby Cartesian pose on all available single-arm joint groups
|
||||
single_arm_groups = robot.info().single_arm_groups
|
||||
if not single_arm_groups:
|
||||
raise RuntimeError("No single-arm joint group found on the connected robot")
|
||||
|
||||
robot_states = robot.states()
|
||||
ik_params_by_group = {}
|
||||
for group in single_arm_groups:
|
||||
pose_to_check = robot_states[group].tcp_pose.copy()
|
||||
pose_to_check[0] += 0.1
|
||||
logger.info(
|
||||
f"[{flexivrdk.kJointGroupNames[group]}] Checking IK feasibility of Cartesian pose {pose_to_check}"
|
||||
)
|
||||
ik_params = flexivrdk.IKParams()
|
||||
ik_params.cartesian_pose = pose_to_check
|
||||
ik_params.seed_q = robot_states[group].q.copy()
|
||||
ik_params.free_orientation = False
|
||||
ik_params_by_group[group] = ik_params
|
||||
|
||||
# Print result
|
||||
result = model.SolveConstrainedIK(ik_params_by_group)
|
||||
logger.info(f"IK result success = {result.success}")
|
||||
for group, q in result.solved_q.items():
|
||||
logger.info(f"[{flexivrdk.kJointGroupNames[group]}] solved_q = {q}")
|
||||
|
||||
except Exception as e:
|
||||
logger.error(str(e))
|
||||
return 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+109
@@ -0,0 +1,109 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
"""utility.py
|
||||
|
||||
Utility methods.
|
||||
|
||||
Import this module as `import utility` and use functions through the module namespace,
|
||||
for example `utility.quat2eulerZYX(...)` and `utility.primitive_state_true_for_groups(...)`.
|
||||
"""
|
||||
|
||||
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
||||
__author__ = "Flexiv"
|
||||
|
||||
import math
|
||||
|
||||
# pip install scipy
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
|
||||
|
||||
def quat2eulerZYX(quat, degree=False):
|
||||
"""
|
||||
Convert quaternion to Euler angles with ZYX axis rotations.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
quat : float list
|
||||
Quaternion input in [w,x,y,z] order.
|
||||
degree : bool
|
||||
Return values in degrees, otherwise in radians.
|
||||
|
||||
Returns
|
||||
----------
|
||||
float list
|
||||
Euler angles in [x,y,z] order, radian by default unless specified otherwise.
|
||||
Returned angles are normalized by scipy to a principal range. In degree mode,
|
||||
this is typically within [-180, 180].
|
||||
"""
|
||||
|
||||
# Convert target quaternion to Euler ZYX using scipy package's 'xyz' extrinsic rotation
|
||||
# NOTE: scipy uses [x,y,z,w] order to represent quaternion. The returned Euler values
|
||||
# are a normalized representation of the same physical orientation.
|
||||
eulerZYX = (
|
||||
R.from_quat([quat[1], quat[2], quat[3], quat[0]])
|
||||
.as_euler("xyz", degrees=degree)
|
||||
.tolist()
|
||||
)
|
||||
|
||||
return eulerZYX
|
||||
|
||||
|
||||
def primitive_state_true_for_groups(primitive_states, state_name):
|
||||
"""
|
||||
Check whether primitive states are true for all groups included in primitive_states.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
primitive_states : dict
|
||||
Primitive states keyed by joint group.
|
||||
state_name : str or dict
|
||||
Primitive state name to check for all groups, or per-group state names keyed by joint
|
||||
group, e.g. "reachedTarget" or {group: "terminated"}.
|
||||
|
||||
Returns
|
||||
----------
|
||||
bool
|
||||
True if all included groups contain the requested primitive state and its integer value is
|
||||
non-zero.
|
||||
"""
|
||||
|
||||
def primitive_state_true(primitive_state, requested_state_name):
|
||||
if requested_state_name not in primitive_state.names_and_values:
|
||||
return False
|
||||
|
||||
value = primitive_state.names_and_values[requested_state_name]
|
||||
return isinstance(value, int) and value != 0
|
||||
|
||||
if isinstance(state_name, dict):
|
||||
return all(
|
||||
group in state_name
|
||||
and primitive_state_true(primitive_state, state_name[group])
|
||||
for group, primitive_state in primitive_states.items()
|
||||
)
|
||||
|
||||
return all(
|
||||
primitive_state_true(primitive_state, state_name)
|
||||
for primitive_state in primitive_states.values()
|
||||
)
|
||||
|
||||
|
||||
def list2str(ls):
|
||||
"""
|
||||
Convert a list to a string.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
ls : list
|
||||
Source list of any size.
|
||||
|
||||
Returns
|
||||
----------
|
||||
str
|
||||
A string with format "ls[0] ls[1] ... ls[n] ", i.e. each value
|
||||
followed by a space, including the last one.
|
||||
"""
|
||||
|
||||
ret_str = ""
|
||||
for i in ls:
|
||||
ret_str += str(i) + " "
|
||||
return ret_str
|
||||
Reference in New Issue
Block a user