Skip to content
Snippets Groups Projects
Commit 2638bea9 authored by Kai Junge's avatar Kai Junge
Browse files

Wrist only teleop working now.

parent ff24207e
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
arm_teleop/avp_teleop_node.py
+ 49
− 72
View file @ 2638bea9
......@@ -17,6 +17,7 @@ from rclpy.node import Node
from copy import deepcopy as cp
from helper_functions.hand_regulator import HandRegulator
from helper_functions.motor_regulator import MotorRegulator
from tf2_ros import TransformBroadcaster
......@@ -47,24 +48,23 @@ class AVPTeleopNode(Node):
# Other
self.hand_regulator = HandRegulator()
self.motor_regulator = MotorRegulator()
# Variables
self.avp_wrist_q = None
self.avp_wrist_t = None
self.static_avp_t = None
self.raw_q = None
self.raw_t = None
self.translation_origin = None
self.current_franka_pose = None
self.static_franka_pose = None
self.translation_demand = None
self.simulated_robot_pose = None
self.left = None
self.right = None
# Parameters
self.is_sim = False
self.current_pitch = None
self.current_yaw = None
self.is_pressed = False
# Static parameters
self.is_sim = True
def gamepad_callback(self, gamepad_raw_msg:Joy):
self.gp.convert_joy_msg_to_dictionary(gamepad_raw_msg)
......@@ -76,8 +76,8 @@ class AVPTeleopNode(Node):
self.is_pressed = False
def hand_wrist_servo_callback(self, msg:Float64MultiArray):
self.left = msg.data[13]
self.right = msg.data[14]
self.current_pitch, y = self.hand_regulator.wrist_motor_pos_to_pitch_yaw(msg.data[13], msg.data[14])
self.current_yaw = -y
def cart_pos_callback(self, msg:Float64MultiArray):
self.current_franka_pose = msg
......@@ -126,13 +126,13 @@ class AVPTeleopNode(Node):
output[3+i] = hand_q[i]
return output
def hand_base_to_wirst(self, hand_base):
ee_m = R.from_quat(hand_base[3:]).as_matrix()
pitch, yaw = self.hand_regulator.wrist_motor_pos_to_pitch_yaw(self.left, self.right)
pitch_m = R.from_euler("z", pitch, degrees=True).as_matrix()
raw_m = R.from_euler("y", yaw, degrees=True).as_matrix()
wrist_m = np.matmul(np.matmul(ee_m, pitch_m), raw_m)
pitch_m = R.from_euler("z", self.current_pitch, degrees=True).as_matrix()
yaw_m = R.from_euler("y", self.current_yaw, degrees=True).as_matrix()
wrist_m = np.matmul(np.matmul(ee_m, pitch_m), yaw_m)
wrist_q = R.from_matrix(wrist_m).as_quat()
return wrist_q
......@@ -169,27 +169,41 @@ class AVPTeleopNode(Node):
return wrist_q, residual_wrt_ee_q
def command_wrist(self, pitch_demand ,yaw_demand):
d_left, d_right = self.hand_regulator.pitch_yaw_to_wrist_motor_pos(pitch_demand, yaw_demand)
demand = {"Wrist_Left":d_left + self.left, "Wrist_Right":d_right + self.right}
limited = self.hand_regulator.check_wrist_pitch_yaw_limit(demand)
def command_wrist(self, extra_pitch ,extra_yaw):
demand_left = limited["Wrist_Left"]
demand_right = limited["Wrist_Right"]
# Determine the pich and yaw that can be applied based on limits
demand_pitch, demand_yaw, _ = self.hand_regulator.check_wrist_pitch_yaw_limit_raw(self.current_pitch + extra_pitch,
-(self.current_yaw + extra_yaw))
# print("current", round(self.current_yaw), " extra", round(extra_yaw))
# print("demand", demand_yaw)
msg = Float64MultiArray()
msg.data = [demand_left, demand_right]
self.wrist_servo_demand_publisher.publish(msg)
# Convert demand pitch/yaw to left/right motor positions
demand_left, demand_right = self.hand_regulator.pitch_yaw_to_wrist_motor_pos(demand_pitch, demand_yaw)
motor_demand = {"Wrist_Left":demand_left, "Wrist_Right":demand_right}
return self.hand_regulator.wrist_motor_pos_to_pitch_yaw(demand_left, demand_right)
# Check if left/right motor positions are fine
motor_demand_limited = self.motor_regulator.check_motor_limits(motor_demand)
# Publish left/right motor positions
if self.gp.button_data["L1"] == 1:
msg = Float64MultiArray()
msg.data = [float(motor_demand_limited["Wrist_Left"]), float(motor_demand_limited["Wrist_Right"])]
self.wrist_servo_demand_publisher.publish(msg)
# Calculate the actual applied pitch and yaw.
applied_pitch, applied_yaw = self.hand_regulator.wrist_motor_pos_to_pitch_yaw(motor_demand_limited["Wrist_Left"], motor_demand_limited["Wrist_Right"])
return applied_pitch, -applied_yaw
def mainloop_callback(self):
timer = time.time()
dt = 0.05
if (self.static_avp_t is None) or (self.left is None):
if (self.static_avp_t is None) or (self.current_pitch is None):
return
# Computes the 6DoF Pose of the hand as of now (based off the measurements of the franka)
if self.is_sim:
starting_franka_pose = cp(self.simulated_robot_pose)
hand_base = self.arm_to_hand_base(self.simulated_robot_pose)
......@@ -197,8 +211,8 @@ class AVPTeleopNode(Node):
starting_franka_pose = cp(self.current_franka_pose.data)
hand_base = self.arm_to_hand_base(self.current_franka_pose.data)
# Computes the orientation by taking into account the wrist pose
current_hand_quat = self.hand_base_to_wirst(hand_base)
current_hand_trans = hand_base[:3]
diff = get_relative_quaternion(current_hand_quat, self.avp_wrist_q).as_euler("zyx", degrees = True)
......@@ -206,58 +220,21 @@ class AVPTeleopNode(Node):
# Get current pitch yaw and see viability
applied_pitch, applied_yaw = self.command_wrist(diff[0], diff[1])
wrist_q, residual_q = self.get_wrist_arm_coord(R.from_quat(current_hand_quat).as_matrix(), applied_pitch, applied_yaw)
# wrist_q, residual_q = self.get_wrist_arm_coord(R.from_quat(hand_base[3:]).as_matrix(), applied_pitch, applied_yaw)
panda_demand = self.hand_to_arm([current_hand_trans[0], current_hand_trans[1], current_hand_trans[2],
residual_q[0], residual_q[1], residual_q[2], residual_q[3]])
# panda_demand = self.hand_to_arm([current_hand_trans[0], current_hand_trans[1], current_hand_trans[2],
# residual_q[0], residual_q[1], residual_q[2], residual_q[3]])
broadcast_tf(self, "world", "robo_coord_wrist", self.translation_demand, self.avp_wrist_q)
# broadcast_tf(self, "world", "robo_coord_wrist", self.translation_demand, self.avp_wrist_q)
broadcast_tf(self, "world", "hand_avp", current_hand_trans, self.avp_wrist_q)
broadcast_tf(self, "world", "hand", current_hand_trans, current_hand_quat)
broadcast_tf(self, "world", "wrist", current_hand_trans, wrist_q)
broadcast_tf(self, "world", "residual", current_hand_trans, residual_q)
broadcast_tf(self, "world", "panda_demand", panda_demand[:3], panda_demand[3:])
broadcast_tf(self, "world", "hand_base", hand_base[:3], hand_base[3:])
if self.is_sim:
broadcast_tf(self, "world", "robot sim", self.simulated_robot_pose[:3], self.simulated_robot_pose[3:])
# broadcast_tf(self, "world", "wrist", current_hand_trans, wrist_q)
# # broadcast_tf(self, "world", "residual", current_hand_trans, residual_q)
# broadcast_tf(self, "world", "panda_demand", panda_demand[:3], panda_demand[3:])
trans_demand = np.asarray(self.translation_demand) + np.asarray(panda_demand[:3]) - np.asarray(starting_franka_pose[:3])
trans_delta = trans_demand - np.asarray(starting_franka_pose[:3])
rot_delta = get_relative_quaternion(panda_demand[3:], starting_franka_pose[3:])
trans_speed = np.linalg.norm(trans_delta)/dt
rot_speed = np.degrees(rot_delta.magnitude())/dt
# base = 0.7; reach = -3; max_damp = 0.95
base = 0.2; reach = -2; max_damp = 0.6
trans_damp = min(base + (1-base) * (1- np.exp(reach * trans_speed)), max_damp)
# base = 0.8; reach = -3; max_damp = 0.95
base = 0.3; reach = -2; max_damp = 0.7
rot_damp = min(base + (1-base) * (1- np.exp(reach * rot_speed)), max_damp)
slerp = Slerp([0, 1], R.from_quat([panda_demand[3:], starting_franka_pose[3:]]))
target_R = slerp([rot_damp])
target_hand_orientation = target_R.as_quat()[0]
franka_demand = []
for i in range(7):
if i < 3:
franka_demand.append(starting_franka_pose[i] * trans_damp + trans_demand[i] * (1-trans_damp))
else:
franka_demand.append(target_hand_orientation[i-3])
# names = ["x", "y", "z", "qx", "qy", "qz", "qw"]
# for name, value in zip(names, franka_demand):
# print(name + ": ", round(value, 4))
if self.gp.button_data["L1"] == 1:
if self.is_sim:
self.simulated_robot_pose = cp(franka_demand)
else:
msg = Float64MultiArray()
msg.data = franka_demand
self.franka_pose_publisher.publish(msg)
print(time.time() - timer)
def main():
rclpy.init()
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment