Closed sherilan closed 1 year ago
zhuyifengzju
commented
1 year ago Thanks for opening up the pull request. Can you also provide a minimal script that controls the robot in this mode so that I can verify your modification is correct? Thanks!
sherilan
commented
1 year ago Here is a smallish script I just ran a couple of times without any issues.
Note: make sure the area in front of the robot is cleared.
It executes a simple grasp operation by:
The table refers to a surface that the robot is placed on top of.
Even though this script relies on absolute poses, it clips target positions and orientations so that they will never be too far away from the current end-effector pose (in terms of euclidean and angular norms). I do this to limit the maximum task-space force that will be generated from the impedance stiffness. For now it is implemented in python, but I intend to move this logic to the c++ franka control loop and include variables for maximum task stiffness force/torque in the controller config. I think it is better to do this on the c++ side since it runs at a much higher frequency, meaning that the maximum force/torque should be enforced in a smoother fashion. Let me know if this is something you're interested in, and we can maybe do another PR later.
import argparse
import json
import deoxys.franka_interface
import deoxys.utils.transform_utils as tfu
import deoxys.experimental.motion_utils as motion_utils
import numpy as np
parser = argparse.ArgumentParser()
parser.add_argument(
'--config',
default='charmander.yml',
help='Path to interface config (with IP addresses, etc)'
)
args = parser.parse_args()
# Configure interface and controller
ROBOT = deoxys.franka_interface.FrankaInterface(args.config, control_freq=20)
CONTROLLER_TYPE = 'OSC_POSE'
CONTROLLER_CONFIG = deoxys.utils.config_utils.get_default_controller_config(CONTROLLER_TYPE)
CONTROLLER_CONFIG['Kp']['translation'] = [400] * 3 # A bit high, but we ramp up and clip position targets
CONTROLLER_CONFIG['Kp']['rotation'] = [400] * 3
CONTROLLER_CONFIG['is_delta'] = False
CONTROLLER_CONFIG['action_scale']['translation'] = 1.0 # <-- IMPORTANT
print(f'Using controller config: {json.dumps(CONTROLLER_CONFIG, indent=4)}')
# Joint reset configuration
JPOS_RESET = [
-0.02120999753845323,
-0.5381841072547232,
0.03466737504764849,
-2.2530796786108223,
-0.032296004055954564,
1.9132555194072929,
0.5933321659996262
]
# Keyframe pose just above grasp pose
POSE_APPROACH = np.array([
[0.9939044369995541, 0.10740864481456847, 0.024456319191703213, 0.4509997119580723],
[0.10717382028626676, -0.9941728009730403, 0.010722087729179848, 0.002519024236947391],
[0.02546545226488054, -0.008035653410015137, -0.9996433990610458, 0.15405420046716645],
[0.0, 0.0, 0.0, 1.0]
])
# Keyframe pose for grasp
POSE_GRASP = np.array([
[0.9942447289200831, 0.10675147788025124, 0.0078923434719399, 0.46235679659031825],
[0.10658951557556717, -0.9941178118661917, 0.01868702555855387, 0.00439591794359747],
[0.00984078681838312, -0.01773823559335698, -0.9997942318248765, 0.033642069500313634],
[0.0, 0.0, 0.0, 1.0]
])
def clip_vector_l2norm(vector, max_norm):
"""Rescales a vector so that its norm is less than `max_norm`"""
assert max_norm > 0
norm = np.linalg.norm(vector)
scale = max_norm / max(norm, max_norm)
return norm, scale * vector
def clip_target_quat(quat_current, quat_target, max_radians):
"""Clips target quat to prevent too large deltas"""
# Compute relative rotation quaternion
if np.dot(quat_target, quat_current) < 0.0:
quat_current = -quat_current
quat_diff = tfu.quat_distance(quat_target, quat_current)
# Convert to rotation vector and clip its max norm
rvec_diff = tfu.quat2axisangle(quat_diff)
angle_radians, rvec_diff_clipped = clip_vector_l2norm(rvec_diff, max_radians)
# Apply clipped difference to original quaternion
quat_target_clipped = tfu.quat_multiply(tfu.axisangle2quat(rvec_diff_clipped), quat_current)
return angle_radians, quat_target_clipped
def clip_target_pos(pos_current, pos_target, max_meters):
"""Clips the target translation to prevent to large deltas"""
# Compute relative translation
pos_diff = pos_target - pos_current
# Clip difference
translation_meters, pos_diff = clip_vector_l2norm(pos_diff, max_meters)
# Update target
pos_target_clipped = pos_current + pos_diff
return translation_meters, pos_target_clipped
def goto_pose(
pose_target,
grasp=False,
tol_meters=5e-3,
tol_radians=5e-2,
max_meters=0.03,
max_radians=0.1,
min_steps=0,
max_steps=100,
rampup=10,
):
print('Going to new pose')
# Convert homogenous pose matrix into a quaternion and translation vector
quat_target = tfu.mat2quat(pose_target[:3, :3])
pos_target = pose_target[:3, 3]
# Loop until we're in the right pose or max_steps is exceeded
for step in range(max_steps):
# Grab current end-effector pose
quat_current, pos_current = ROBOT.last_eef_quat_and_pos
pos_current = pos_current.reshape(-1)
# Clip targets
max_scale = min(1, (step + 1) / rampup)
diff_radians, quat_target_clipped = clip_target_quat(
quat_current=quat_current, quat_target=quat_target, max_radians=max_radians * max_scale
)
diff_meters, pos_target_clipped = clip_target_pos(
pos_current=pos_current, pos_target=pos_target, max_meters=max_meters * max_scale
)
# Convert new target quat to rotation vector
rvec_target_clipped = tfu.quat2axisangle(quat_target_clipped)
# Maybe stop early if pos/rot tolerances are met
print(f'Step={step} err_radians={diff_radians :.3f} err_meters={diff_meters :.4f} max_scale={max_scale}')
if step >= min_steps and diff_radians <= tol_radians and diff_meters <= tol_meters:
print('Pose reached within configured tolerances')
return
# Otherwise, move towards clipped targets
grasp_action = 1 if grasp else -1
action = pos_target_clipped.tolist() + rvec_target_clipped.tolist() + [grasp_action]
ROBOT.control(CONTROLLER_TYPE, action, CONTROLLER_CONFIG)
print('Warning: failed to reach target pose within tolerance')
MSG = 'Robot is about to move. Make sure the area in front of it is cleared. Ready? [y] '
if input(MSG).lower() in ['y', 'yes']:
motion_utils.reset_joints_to(ROBOT, JPOS_RESET, timeout=3)
goto_pose(POSE_APPROACH, grasp=False)
goto_pose(POSE_GRASP, grasp=False)
goto_pose(POSE_GRASP, grasp=True, min_steps=20)
goto_pose(POSE_APPROACH, grasp=True)
goto_pose(POSE_GRASP, grasp=True)
goto_pose(POSE_GRASP, grasp=False, min_steps=20)
goto_pose(POSE_APPROACH, grasp=False)
Note: I'm brand new to this framework, so please make sure that this change makes sense to you too. I've been running this code for a couple of days (with is_delta=False and action_scale.translation=1.0 configured) and it seems to work well.
-- original commit message --
UT-Austin-RPL/deoxys_control#3
Removes the term that adds the current end-effector position to the translation action in the section of ComputeGoal that is exectued if
is_delta=False.