Not working with full referential (orientation mode "all") with unity orientation
Miker2808 opened this issue · 5 comments
Miker2808 commented
Tried quaternions and eulers, in any possible combination, it always works on one axis and died completely in another when the orientation comes from unity, (both quaternions and eulers) if you have a suggestion or would like to make it a possibility to insert quaternions instead of rotation matrixes it would be great, so far it seems the IK works only for position and not orientation.
I attempted rotation translation using scipy but with no luck.
Phylliade commented
Hello @Miker2808, can you copy paste the code you're using?
Miker2808 commented
Sorry if my issue was a little aggressive, I really lost my mind over this orientation. unity conflicting with my URDF made it really difficult with shifting orientation.
I'm including the full script, it communicates using ROS2 (ROS2 coms from unity straight away as Vector3 or Quaternion) depending on the script, I can include a quaternion script as well.
my Unity is Y axis is up, while the URDF is Z axis is up.
Sidenode: the wiki with the "full orientation" has a "sample" which says "Look at the sample below" and right under it there is nothing.
import numpy as np
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink
from ikpy.inverse_kinematics import inverse_kinematic_optimization
import ikpy.utils.plot as plot_utils
import matplotlib.pyplot as plt
from urllib3 import Retry
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Vector3
from scipy.spatial.transform import Rotation
hdt_chain = Chain(name="Hdt_Adroit_Chain", active_links_mask=[1,0,1,0,0,1,0,0,1,0,1,1,0], links=[
#URDFLink(
# name="integration_kit_joint",
# origin_translation=[0.0, 0.0, 0.0],
# origin_orientation=[0.0, 0.0 ,0.0],
# joint_type="fixed"
#),
URDFLink(
name="joint1",
origin_translation=[-0.1, 0.0, 0.25],
origin_orientation=[0.0, 0.0 ,0.0],
rotation=[0,0,1],
bounds=[-2.7,2.7],
joint_type="revolute"
),
URDFLink(
name="joint1f",
origin_translation=[0.0, 0.0, 0.09217],
origin_orientation=[0.0, 0.0 ,0.0],
joint_type="fixed"
),
URDFLink(
name="joint2",
origin_translation=[0.0, 0.0, 0.07],
origin_orientation=[0.0, 0.0 ,0.0],
rotation=[1,0,0],
bounds=[-2.0,2.0],
joint_type="revolute"
),
URDFLink(
name="joint2f",
origin_translation=[0.0, 0.0, 0.077],
origin_orientation=[0.0, 0.0 ,0.0],
joint_type="fixed"
),
URDFLink(
name="joint3f",
origin_translation=[0.0, 0.0, 0.2530],
origin_orientation=[0.0, 0.0 ,0.0],
joint_type="fixed"
),
URDFLink(
name="joint4",
origin_translation=[0.0, 0.0, 0.07],
origin_orientation=[0.0, 0.0 ,0.0],
rotation=[1,0,0],
bounds=[-1.57,1.57],
joint_type="revolute"
),
URDFLink(
name="wire_cover_joint",
origin_translation=[0.0, -0.0363, 0.074],
origin_orientation=[0.0, 0.0 ,0.0],
joint_type="fixed"
),
URDFLink(
name="accessory_puck_joint",
origin_translation=[0.0, -0.0413, 0.074],
origin_orientation=[0.0, 0.0 ,0.0],
joint_type="fixed"
),
URDFLink(
name="joint5",
origin_translation=[0.0, -0.0425, 0.00],
origin_orientation=[0.0, 0.0 ,0.0],
rotation=[0,-1,0],
bounds=[-2.1,2.1],
joint_type="revolute"
),
URDFLink(
name="joint5f",
origin_translation=[0.0, -0.3, 0.00],
origin_orientation=[0.0, 0.0 ,0.0],
joint_type="fixed"
),
URDFLink(
name="joint6",
origin_translation=[0.0, -0.055, 0.00],
origin_orientation=[0.0, 0.0 ,0.0],
rotation=[1,0,0],
bounds=[-1.57,1.57],
joint_type="revolute"
),
URDFLink(
name="joint7",
origin_translation=[0.0, -0.0771, 0.00],
origin_orientation=[0.0, 0.0 ,0.0],
rotation=[0,0,1],
bounds=[-2.1,2.1],
joint_type="revolute"
),
URDFLink(
name="endpoint",
origin_translation=[0.0, -0.055, 0.00],
origin_orientation=[0.0, 0.0 ,0.0],
joint_type="fixed"
)])
def deg_minus_ninety(degree: int):
new_deg = degree - 90
if(new_deg < 0):
new_deg = new_deg + 360
return new_deg
def reverse_axis(degree :int):
return abs(360 - degree)
def half_power_axis(degree :int):
if (degree < 180):
return degree / 2
else:
new_deg = degree - 270
return 270 + new_deg / 2
class AdroitIKPublisher(Node):
def __init__(self, target_velocity = 1.0):
super().__init__('HDTArmIKPublisher')
self.pub_joint = self.create_publisher(JointState, "/UnityVR/hdt_adroit_coms/joint_cmd", 10)
self.sub_righthand_rpy = self.create_subscription(Vector3, "/UnityVR/RightHand/Rotation/Degrees",self.righthand_rpy_callback, 10)
self.sub_righthand_pos = self.create_subscription(Vector3, "/UnityVR/RightHand/Position", self.righthand_pos_callback, 10)
self.sub_orientation_x = self.create_subscription(Vector3, "/orientation/x", self.orientation_x_callback, 10)
self.sub_orientation_y = self.create_subscription(Vector3, "/orientation/y", self.orientation_y_callback, 10)
self.sub_orientation_z = self.create_subscription(Vector3, "/orientation/z", self.orientation_z_callback, 10)
self.timer = self.create_timer(0.1, self.timer_callback)
self.frame_id = 1
self.target_velocity = target_velocity
self.joints_error_list = [0.00, 0.0, -0.30, 0.0, 0.0, 0.00, 0.0, 0.0, 0.00, 0.0, -0.35, 0.00, 0.0]
self.count_joint = [1,0,1,0,0,1,0,0,1,0,1,1,0]
self.starting_pos = [0.00, 0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.00, 0.0, 0.0, 0.00, 0.0]
self.latest_pos = [0.00, 0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.00, 0.0, 0.0, 0.00, 0.0]
self.righthand_pos_msg = Vector3()
self.righthand_rpy_msg = Vector3()
self.orientation_x_msg = Vector3()
self.orientation_y_msg = Vector3()
self.orientation_z_msg = Vector3()
def construct_jointstate_msg(self, ik_output):
## Constructs a pos message, dont touch
new_position = [0.0]
for index in range(len(ik_output)):
if(self.count_joint[index]):
new_position.append(ik_output[index] + self.joints_error_list[index])
new_position.extend([0.0,0.0,0.0])
joint_msg = JointState()
joint_msg.header.frame_id = str(self.frame_id)
joint_msg.name = ['index_prox', 'joint1', 'joint2', 'joint4', 'joint5', 'joint6', 'joint7', 'ring_prox', 'thumb_base', 'thumb_prox']
joint_msg.velocity = [float(self.target_velocity),float(self.target_velocity),float(self.target_velocity),float(self.target_velocity),float(self.target_velocity),float(self.target_velocity),float(self.target_velocity),float(self.target_velocity),float(self.target_velocity),float(self.target_velocity)]
joint_msg.effort = [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0]
joint_msg.position = new_position
return joint_msg
def calculate_target_frame(self):
#target_frame = np.eye(4)
#target_position = [self.righthand_pos_msg.x -0.1, -1 * self.righthand_pos_msg.z, self.righthand_pos_msg.y]
#target_frame[:3, -1] = target_position
#target_orientation = [self.righthand_rpy_msg.x, -1 * self.righthand_rpy_msg.z, self.righthand_rpy_msg.y]
#target_orientation = [0.0, 0.0, 0.0]
#target_orientation[0] = -1 * np.rad2deg(np.arcsin(self.righthand_pos_msg.x / (self.righthand_pos_msg.z + 0.0001)))/90 * self.righthand_rpy_msg.x
#target_orientation[1] = -1 * np.rad2deg(np.arcsin(self.righthand_pos_msg.z / (self.righthand_pos_msg.x + 0.0001)))/90 * self.righthand_rpy_msg.z
#target_orientation[2] = np.rad2deg(np.arctan(self.righthand_pos_msg.x / (self.righthand_pos_msg.z + 0.0001))) - self.righthand_rpy_msg.y
print(np.rad2deg(np.arctan(self.righthand_pos_msg.x / self.righthand_pos_msg.z)))
#orientation = Rotation.from_euler('XYZ', target_orientation, degrees=True).as_matrix()
#target_frame[:3, :3] = orientation
def timer_callback(self):
try:
#eval_input = eval(input("Insert position, orientation UNITY: ((x,y,z),(r,p,y)): "))
target_frame = np.eye(4)
target_position = [self.righthand_pos_msg.x -0.1, -1 * self.righthand_pos_msg.z, self.righthand_pos_msg.y]
target_frame[:3, -1] = target_position
#target_orientation = [self.righthand_rpy_msg.x, -1 * self.righthand_rpy_msg.z, self.righthand_rpy_msg.y]
#TODO: Check what orientation is required for each vector
target_orientation_new = [0.0, 0.0, 0.0]
target_orientation_new[0] = self.righthand_rpy_msg.z
target_orientation_new[1] = -1 * self.righthand_rpy_msg.x
#target_orientation_new[2] = np.rad2deg(np.arctan(self.righthand_pos_msg.x / (self.righthand_pos_msg.z + 0.0001))) - self.righthand_rpy_msg.y
target_orientation_new[2] = -1 * self.righthand_rpy_msg.y
#print(np.rad2deg(np.arctan(self.righthand_pos_msg.x / (self.righthand_pos_msg.z+ 0.0001))))
#print("URDF -X:" + str(target_orientation_new[0]) + " Y:" + str(target_orientation_new[1]) + " Z:" + str(target_orientation_new[2]) + " | | ", end="")
#print("UNITY-X:" + str(target_orientation_new[1]) + " Y:" + str(target_orientation_new[2]) + " Z:" + str(target_orientation_new[0]) + " | | ", end="")
#print("RAW X:" + str(self.righthand_rpy_msg.x) + " Y:" + str(self.righthand_rpy_msg.y) + " Z:" + str(self.righthand_rpy_msg.z))
orientation = Rotation.from_euler('XYZ', target_orientation_new, degrees=True).as_matrix()
target_frame[:3, :3] = orientation
hdt_ik = inverse_kinematic_optimization(hdt_chain, target_frame, self.starting_pos, orientation_mode="all", optimization_method="SLSQP")
joint_msg = self.construct_jointstate_msg(hdt_ik)
self.pub_joint.publish(joint_msg)
#self.get_logger().info('Publishing: "%s"' % joint_msg.position)
self.frame_id += 1 % 1000
except Exception as e:
print(e)
def righthand_rpy_callback(self, msg):
self.righthand_rpy_msg = msg
def righthand_pos_callback(self, msg):
self.righthand_pos_msg = msg
def orientation_x_callback(self,msg):
self.orientation_x_msg = msg
def orientation_y_callback(self,msg):
self.orientation_y_msg = msg
def orientation_z_callback(self,msg):
self.orientation_z_msg = msg
joints_error_list = [0.0, 0.00, 0.0, -0.30, 0.0, 0.0, 0.00, 0.0, 0.0, 0.00, 0.0, -0.35, 0.00, 0.0]
count_joint = [0,1,0,1,0,0,1,0,0,1,0,1,1,0]
def main(args=None):
rclpy.init(args=args)
joint_publisher = AdroitIKPublisher(target_velocity=1.0)
rclpy.spin(joint_publisher)
joint_publisher.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
Phylliade commented
Here is a working example of the full orientation mode:
def test_orientation_full_frame(baxter_left_arm):
# Found by exploring the reachable values
target_position = [0.1, 0.5, -0.1]
target_orientation = [[0,0,1], [1,0,0], [0,1,0]]
# Begin to place the arm in the right position
ik = baxter_left_arm.inverse_kinematics(target_position)
ik = baxter_left_arm.inverse_kinematics(target_position, target_orientation, initial_position=ik, orientation_mode='all')
position = baxter_left_arm.forward_kinematics(ik)[:3, 3]
orientation = baxter_left_arm.forward_kinematics(ik)[:3, :3]
# Check
np.testing.assert_almost_equal(orientation, target_orientation, decimal=5)
np.testing.assert_almost_equal(position, target_position, decimal=5)
Phylliade commented
stale commented
This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.