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

Arm/wrist control for orientation.

parent 19516669
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
+ 78
− 38
View file @ fe42b561
......@@ -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,21 +22,27 @@ 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)
# 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())
......@@ -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/imu_suite_node.py
+ 35
− 17
View file @ fe42b561
......@@ -59,18 +59,35 @@ class OrientationNode(Node):
self.calib_q = [None, None, None, None]
def imu_callback(self, raw):
for i, which_sensor in enumerate([0, 1, 2, 3]):
q = np.array(
[
raw.data[which_sensor*4],
raw.data[which_sensor*4 + 1],
raw.data[which_sensor*4 + 2],
raw.data[which_sensor*4 + 3]
],
dtype=float) / 10000
imu1 = np.array([raw.data[0], raw.data[1], raw.data[2], raw.data[3]]) / 10000
# imu2 = np.array([raw.data[4], raw.data[5], raw.data[6], raw.data[7]]) / 10000
# rotate_z = R.from_euler("z", 180, degrees=True)
# q0 = rotate_z.as_quat()
# x = get_series_quaternion(imu2, q0)
# broadcast_tf(self, "world", "imu1", [0.0, 0.0, 0.0], imu1)
# broadcast_tf(self, "world", "imu2", [0.0, 0.0, 0.0], x)
# wrist = (imu1 + x) / 2
# broadcast_tf(self, "world", "wrist", [0.0, 0.0, 0.0], wrist)
# print("imu1", imu1)
# print("imu2", x)
# print("wrist", wrist)
# for i, which_sensor in enumerate([0, 1]):
# q = np.array(
# [
# raw.data[which_sensor*4],
# raw.data[which_sensor*4 + 1],
# raw.data[which_sensor*4 + 2],
# raw.data[which_sensor*4 + 3]
# ],
# dtype=float) / 10000
self.raw_q[i] = q
broadcast_tf(self, "world", "imu" + str(i), [0.0, 0.0, i * 0.1], q)
# self.raw_q[i] = q
# broadcast_tf(self, "world", "imu" + str(i), [0.0, 0.0, i * 0.1], q)
# try:
# rel = get_relative_quaternion(self.raw_q[0], self.raw_q[1]).as_quat()
......@@ -100,13 +117,14 @@ class OrientationNode(Node):
def mainloop_callback(self):
if self.key.keyPress == "e":
self.zero_shoulder = cp(self.raw_q[1])
self.zero_shoulder[3] *= -1
pass
# if self.key.keyPress == "e":
# self.zero_shoulder = cp(self.raw_q[1])
# self.zero_shoulder[3] *= -1
rel = get_relative_quaternion(self.raw_q[2], self.raw_q[1]).as_quat()
self.zero_elbow = cp(rel)
self.zero_elbow[3] *= -1
# rel = get_relative_quaternion(self.raw_q[2], self.raw_q[1]).as_quat()
# self.zero_elbow = cp(rel)
# self.zero_elbow[3] *= -1
# r = R.from_quat(self.raw_q[2])
......
arm_teleop/orientation_node.py
+ 85
− 39
View file @ fe42b561
......@@ -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