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

Reverted back to before.

parent feda338c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
arm_teleop/arm_teleop_node.py
+ 83
− 43
View file @ bbb22575
......@@ -4,7 +4,8 @@ from helper_functions.gamepad_functions import GamepadFunctions
from .teleop_helper_functions import posestamp_message_as_str
import time
from sensor_msgs.msg import Joy
from std_msgs.msg import Float64MultiArray
from helper_functions.hand_regulator import HandRegulator
from .teleop_helper_functions import *
import rclpy
......@@ -21,31 +22,37 @@ class ArmTeleopNode(Node):
self.gp = GamepadFunctions()
self.gamepad_subscriber = self.create_subscription(Joy, '/gamepad', self.gamepad_callback, 10)
# Hand regulator
self.hand_regulator = HandRegulator()
# Main loop
self.mainloop = self.create_timer(0.001, self.mainloop_callback)
# Translation loop
self.translation_loop = self.create_timer(0.01, self.translation_callback)
# Publisher
self.franka_pose_publisher = self.create_publisher(PoseStamped, '/equilibrium_pose', 10)
self.franka_pose_publisher = self.create_publisher(Float64MultiArray, '/franka/pose_demand', 10)
self.wrist_servo_pos_publisher = self.create_publisher(Float64MultiArray, '/wrist/servo_demand', 10)
# Subscriber
self.franka_cartesian_pose_subscriber = self.create_subscription(PoseStamped, '/cartesian_pose', self.cart_pos_callback, 10)
self.target_translation_subscriber = self.create_subscription(PoseStamped, '/franka/translation_demand', self.translation_callback, 10)
self.target_orientation_subscriber = self.create_subscription(PoseStamped, '/franka/orientation_demand',self.orientation_callback, 10)
self.franka_cartesian_pose_subscriber = self.create_subscription(Float64MultiArray, '/franka/pose', self.cart_pos_callback, 10)
self.target_translation_subscriber = self.create_subscription(Float64MultiArray, '/franka/translation_demand', self.translation_callback, 10)
self.target_orientation_subscriber = self.create_subscription(Float64MultiArray, '/franka/orientation_demand',self.orientation_callback, 10)
self.hand_wrist_servo_position_subscriber = self.create_subscription(Float64MultiArray, '/hand_wrist/servo_positions',self.hand_wrist_servo_callback, 10)
# Variables
self.translation_demand = None
self.orientation_demand = None
self.current_franka_pose = None
self.current_hand_wrist_servo_pos = None
self.last_translation_update = cp(time.time())
self.last_orientation_update = cp(time.time())
self.translation_delta = [0.0, 0.0, 0.0]
# Flags
self.is_active = False
def check_timer(self, which_axis, threshold = 0.5):
if which_axis == "translation":
pass
which_timer = self.last_translation_update
else:
which_timer = self.last_orientation_update
......@@ -54,51 +61,84 @@ class ArmTeleopNode(Node):
else:
return False
def translation_callback(self, msg:PoseStamped):
def translation_callback(self, msg:Float64MultiArray):
self.translation_demand = msg
self.last_translation_update = cp(time.time())
def orientation_callback(self, msg:PoseStamped):
def orientation_callback(self, msg:Float64MultiArray):
self.orientation_demand = msg
self.last_orientation_update = cp(time.time())
def cart_pos_callback(self, msg:PoseStamped):
def cart_pos_callback(self, msg:Float64MultiArray):
self.current_franka_pose = msg
def hand_wrist_servo_callback(self, msg:Float64MultiArray):
self.current_hand_wrist_servo_pos = msg
def gamepad_callback(self, gamepad_raw_msg:Joy):
self.gp.convert_joy_msg_to_dictionary(gamepad_raw_msg)
def mainloop_callback(self):
if self.current_franka_pose is not None:
if self.gp.button_data["L1"] == 1 and self.is_active == False:
self.initial_pose = cp(self.current_franka_pose)
self.is_active = True
self.waiting_timer = cp(time.time())
if self.gp.button_data["L1"] == 0:
self.is_active = False
if self.is_active and (time.time() - self.waiting_timer > 0.5):
target_pose = cp(self.initial_pose)
print_msg = "\n"
if (self.translation_demand is not None) and self.check_timer("translation"):
print_msg += "Translation "
target_pose.pose.position = cp(self.translation_demand.pose.position)
if (self.orientation_demand is not None) and self.check_timer("orientation"):
print_msg += "Orientation "
target_pose.pose.orientation = cp(self.orientation_demand.pose.orientation)
target_pose.pose.position.x += self.orientation_demand.pose.position.x
target_pose.pose.position.y += self.orientation_demand.pose.position.y
target_pose.pose.position.z += self.orientation_demand.pose.position.z
print_msg += "Publish! "
self.franka_pose_publisher.publish(target_pose)
print_msg += ("\n" + posestamp_message_as_str(target_pose))
self.get_logger().info(print_msg, throttle_duration_sec = 0.1)
if (self.current_franka_pose is None) or (self.current_hand_wrist_servo_pos is None):
return
if self.gp.button_data["L1"] == 1 and self.is_active == False:
self.initial_pose = list(cp(self.current_franka_pose.data))
self.initial_pose_wrist = [self.current_hand_wrist_servo_pos.data[13], self.current_hand_wrist_servo_pos.data[14]]
self.is_active = True
self.waiting_timer = cp(time.time())
if self.gp.button_data["L1"] == 0:
self.is_active = False
if self.is_active and (time.time() - self.waiting_timer > 0.5):
target_pose = cp(self.initial_pose)
target_wrist = cp(self.initial_pose_wrist)
print_msg = "\n"
if (self.translation_demand is not None) and self.check_timer("translation"):
print_msg += "Translation "
for i in range(3):
target_pose[i] = cp(self.translation_demand[i])
if (self.orientation_demand is not None) and self.check_timer("orientation"):
print_msg += "Orientation "
for i in range(4):
target_pose[i+3] = cp(self.orientation_demand.data[i+3])
for i in range(3):
target_pose[i] += self.orientation_demand.data[i]
# Wrist!
# Combine motor commands to pitch + yaw
pitch, yaw = self.hand_regulator.wrist_motor_pos_to_pitch_yaw(target_wrist[0], target_wrist[1])
pitch += cp(self.orientation_demand.data[7])
yaw += cp(self.orientation_demand.data[8])
left, right = self.hand_regulator.pitch_yaw_to_wrist_motor_pos(pitch, yaw)
# Wrist
wrist_msg = Float64MultiArray()
wrist_msg.data = [left, right]
self.wrist_servo_pos_publisher.publish(wrist_msg)
print_msg += "Publish! "
# Arm
arm_msg = Float64MultiArray()
arm_msg.data = target_pose
self.franka_pose_publisher.publish(arm_msg)
for t in target_pose:
print(round(t, 2), end = ", ")
# print([self.current_hand_wrist_servo_pos.data[13], self.current_hand_wrist_servo_pos.data[14]])
# print(self.orientation_demand.data[7], self.orientation_demand.data[8])
print(left, right)
print("")
# print_msg += ("\n" + posestamp_message_as_str(target_pose))
# self.get_logger().info(print_msg, throttle_duration_sec = 0.1)
def main():
rclpy.init()
......
arm_teleop/orientation_node.py
+ 85
− 39
View file @ bbb22575
......@@ -4,12 +4,12 @@ from helper_functions.gamepad_functions import GamepadFunctions
import numpy as np
from sensor_msgs.msg import Joy
from std_msgs.msg import Float32MultiArray
from std_msgs.msg import Float32MultiArray, Int32MultiArray, Float64MultiArray
from .teleop_helper_functions import broadcast_tf, get_relative_quaternion, get_series_quaternion
from scipy.spatial.transform import Rotation as R
from scipy.spatial.transform import Slerp
from .teleop_helper_functions import broadcast_tf
import rclpy
from rclpy.node import Node
......@@ -36,47 +36,65 @@ class OrientationNode(Node):
# Main loop
self.mainloop = self.create_timer(0.001, self.mainloop_callback)
self.calc_loop = self.create_timer(0.035, self.calcloop_callback)
# Subscriber
self.manus_skeleton_subscriber = self.create_subscription(Float32MultiArray, '/manus/imu', self.imu_callback, 10)
self.franka_cartesian_pose_subscriber = self.create_subscription(PoseStamped, '/cartesian_pose', self.cart_pos_callback, 10)
self.manus_skeleton_subscriber = self.create_subscription(Float32MultiArray, '/manus/imu', self.manus_imu_callback, 10)
self.franka_cartesian_pose_subscriber = self.create_subscription(Float64MultiArray, '/franka/pose', self.cart_pos_callback, 10)
self.imu_subscriber = self.create_subscription(Int32MultiArray, '/raw_imu_data', self.wrist_imu_callback, 1)
# Publisher
self.target_orientation_publisher = self.create_publisher(PoseStamped, '/franka/orientation_demand', 10)
self.target_orientation_publisher = self.create_publisher(Float64MultiArray, '/franka/orientation_demand', 10)
# Variables
self.calibration_stage = 0
self.target_hand_orientation = None
self.current_franka_pose = None
self.calibration_timer = cp(time.time())
self.manus_q = None
self.wrist_q = None
self.wrist_base = None
self.palm_base = None
# Parameters
self.DAMPING = 0.95
def cart_pos_callback(self, msg:PoseStamped):
def cart_pos_callback(self, msg:Float64MultiArray):
self.current_franka_pose = msg
def manus_imu_callback(self, msg:Float32MultiArray):
self.manus_q = msg.data
def imu_callback(self, msg:Float32MultiArray):
q = msg.data
rotate_z = R.from_euler("z", -45, degrees=True)
def wrist_imu_callback(self, raw: Int32MultiArray):
self.wrist_q = np.array([raw.data[0], raw.data[1], raw.data[2], raw.data[3]], dtype=float) / 10000
def calcloop_callback(self):
if (self.manus_q is None) or (self.wrist_q is None):
return
rotate_z = R.from_euler("z", 45, degrees=True)
q0 = rotate_z.as_quat()
broadcast_tf(self, "panda_link8", "temp1", [0.0, 0.0, 0.1], q0)
rotate_y = R.from_euler("y", 30, degrees=True)
q1 = rotate_y.as_quat()
broadcast_tf(self, "temp1", "temp2", [0.0, 0.0, 0.0], q1)
rotate_z90 = R.from_euler("z", -90, degrees=True)
q2 = rotate_z90.as_quat()
broadcast_tf(self, "temp2", "hand_frame", [0.0, 0.0, 0.0], q2)
q1 = rotate_z90.as_quat()
broadcast_tf(self, "temp1", "hand_frame", [0.0, 0.0, 0.0], q1)
qp = cp(self.manus_q)
qw = cp(self.wrist_q)
# print(qw)
# Start of calibration
if self.calibration_stage == 1 or self.calibration_stage == 2:
broadcast_tf(self, "hand_frame", "new_base", [0.0, 0.0, 0.0], [q[0], q[1], q[2], q[3]*-1])
self.wrist_base = np.array([qw[0], qw[1], qw[2], qw[3]*-1], dtype = float)
self.palm_base = np.array([qp[0], qp[1], qp[2], qp[3]*-1], dtype = float)
broadcast_tf(self, "hand_frame", "new_base", [0.0, 0.0, 0.0], [qw[0], qw[1], qw[2], qw[3]*-1])
broadcast_tf(self, "hand_frame", "new_base_palm", [0.0, 0.0, 0.0], [qp[0], qp[1], qp[2], qp[3]*-1])
try:
t = self.tf_buffer.lookup_transform("world", "new_base", rclpy.time.Time())
tp = self.tf_buffer.lookup_transform("world", "new_base_palm", rclpy.time.Time())
ee_to_hand = self.tf_buffer.lookup_transform("hand_frame", "panda_link8", rclpy.time.Time())
self.ee_to_hand_trans = [ee_to_hand.transform.translation.x, ee_to_hand.transform.translation.y, ee_to_hand.transform.translation.z]
self.ee_to_hand_ori = [ee_to_hand.transform.rotation.x, ee_to_hand.transform.rotation.y, ee_to_hand.transform.rotation.z, ee_to_hand.transform.rotation.w]
......@@ -84,26 +102,31 @@ class OrientationNode(Node):
print("[Step 1] Waiting for new_base broadcast")
return
self.world_to_glove_root_trans = [t.transform.translation.x, t.transform.translation.y, t.transform.translation.z]
self.world_to_glove_root_ori = [t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w]
self.target_hand_orientation = cp(q)
self.world_to_wrist_root_trans = [t.transform.translation.x, t.transform.translation.y, t.transform.translation.z]
self.world_to_wrist_root_ori = [t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w]
self.world_to_palm_root_ori = [tp.transform.rotation.x, tp.transform.rotation.y, tp.transform.rotation.z, tp.transform.rotation.w]
self.target_hand_orientation = cp(qw)
self.static_franka_pose = cp(self.current_franka_pose)
self.calibration_stage += 1
# Mid calibration
if self.calibration_stage >= 3:
broadcast_tf(self, "world", "new_base_wrt_world", self.world_to_glove_root_trans, self.world_to_glove_root_ori)
broadcast_tf(self, "new_base_wrt_world", "imu_wrt_world", [0.0, 0.0, 0.0], q)
broadcast_tf(self, "imu_wrt_world", "ee_pos", self.ee_to_hand_trans, self.ee_to_hand_ori)
if self.calibration_stage >= 3:
broadcast_tf(self, "world", "new_base_wrt_world", self.world_to_wrist_root_trans, self.world_to_wrist_root_ori)
broadcast_tf(self, "world", "new_base_palm_wrt_world", self.world_to_wrist_root_trans, self.world_to_palm_root_ori)
broadcast_tf(self, "new_base_wrt_world", "wrist_wrt_world", [0.0, 0.0, 0.0], qw)
broadcast_tf(self, "wrist_wrt_world", "ee_pos", self.ee_to_hand_trans, self.ee_to_hand_ori)
broadcast_tf(self, "new_base_palm_wrt_world", "palm_wrt_world", [0.0, 0.0, 0.0], qp)
# Interpolate hand orientation
if self.target_hand_orientation is None:
self.target_hand_orientation = cp(q)
self.target_hand_orientation = cp(qw)
else:
# Apply damping by linear interpolation
slerp = Slerp([0, 1], R.from_quat([q, self.target_hand_orientation]))
slerp = Slerp([0, 1], R.from_quat([qw, self.target_hand_orientation]))
target_R = slerp([self.DAMPING])
target_hand_orientation = target_R.as_quat()[0]
......@@ -123,19 +146,42 @@ class OrientationNode(Node):
# If all processes are ready, get the arm position, and update the
if self.calibration_stage == 4:
target_pose = cp(self.current_franka_pose)
target_pose.pose.position.x = target_wrt_world.transform.translation.x - self.static_franka_pose.pose.position.x
target_pose.pose.position.y = target_wrt_world.transform.translation.y - self.static_franka_pose.pose.position.y
target_pose.pose.position.z = target_wrt_world.transform.translation.z - self.static_franka_pose.pose.position.z
target_pose = list(cp(self.current_franka_pose.data))
target_pose[0] = target_wrt_world.transform.translation.x - self.static_franka_pose.data[0]
target_pose[1] = target_wrt_world.transform.translation.y - self.static_franka_pose.data[1]
target_pose[2] = target_wrt_world.transform.translation.z - self.static_franka_pose.data[2]
target_pose.pose.orientation.x = target_wrt_world.transform.rotation.x
target_pose.pose.orientation.y = target_wrt_world.transform.rotation.y
target_pose.pose.orientation.z = target_wrt_world.transform.rotation.z
target_pose.pose.orientation.w = target_wrt_world.transform.rotation.w
target_pose[3] = target_wrt_world.transform.rotation.x
target_pose[4] = target_wrt_world.transform.rotation.y
target_pose[5] = target_wrt_world.transform.rotation.z
target_pose[6] = target_wrt_world.transform.rotation.w
self.target_orientation_publisher.publish(target_pose)
# Difference between wrist and palm
try:
wrist_calib = self.tf_buffer.lookup_transform("world", "wrist_wrt_world", rclpy.time.Time())
palm_calib = self.tf_buffer.lookup_transform("world", "palm_wrt_world", rclpy.time.Time())
wrist_calib_q = [wrist_calib.transform.rotation.x, wrist_calib.transform.rotation.y, wrist_calib.transform.rotation.z, wrist_calib.transform.rotation.w]
palm_calib_q = [palm_calib.transform.rotation.x, palm_calib.transform.rotation.y, palm_calib.transform.rotation.z, palm_calib.transform.rotation.w]
print([round(target_pose.pose.position.x, 4), round(target_pose.pose.position.y, 4), round(target_pose.pose.position.z, 4)])
diff = get_relative_quaternion(wrist_calib_q, palm_calib_q).as_euler("zyx", degrees = True)
for d in diff:
print(round(d, 2), end = ", ")
print("")
target_pose.append(diff[2])
target_pose.append(diff[1] * -1)
out_msg = Float64MultiArray()
out_msg.data = target_pose
self.target_orientation_publisher.publish(out_msg)
except:
pass
def gamepad_callback(self, gamepad_raw_msg:Joy):
self.gp.convert_joy_msg_to_dictionary(gamepad_raw_msg)
......
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