yumi robot go to pose issue

[fourteenjiang]

Hi:

I have been having an issue with using command go_to pose, any command related with pose will fail, I tried to print if reachable for different positions, even the home position is unreachable, the coder and error message are as follow, I would be appreciating if you could have a look.

import numpy as np
from yumipy import YuMiRobot, YuMiState, YuMiArm
from autolab_core import RigidTransform

def quaternion_rotation_matrix(Q):

# Covert a quaternion into a full three-dimensional rotation matrix.
#
# Input
# :param Q: A 4 element array representing the quaternion (q0,q1,q2,q3)
#
# Output
# :return: A 3x3 element matrix representing the full 3D rotation matrix.
#          This rotation matrix converts a point in the local reference
#          frame to a point in the global reference frame.

# Extract the values from Q
q0 = Q[0]
q1 = Q[1]
q2 = Q[2]
q3 = Q[3]

# First row of the rotation matrix
r00 = 2 * (q0 * q0 + q1 * q1) - 1
r01 = 2 * (q1 * q2 - q0 * q3)
r02 = 2 * (q1 * q3 + q0 * q2)

# Second row of the rotation matrix
r10 = 2 * (q1 * q2 + q0 * q3)
r11 = 2 * (q0 * q0 + q2 * q2) - 1
r12 = 2 * (q2 * q3 - q0 * q1)

# Third row of the rotation matrix
r20 = 2 * (q1 * q3 - q0 * q2)
r21 = 2 * (q2 * q3 + q0 * q1)
r22 = 2 * (q0 * q0 + q3 * q3) - 1

# 3x3 rotation matrix
rot_matrix = np.array([[r00, r01, r02],
                       [r10, r11, r12],
                       [r20, r21, r22]])

return rot_matrix

def trans_matrix(T):
T0 = T[0]*0.001
T1 = T[1]*0.001
T2 = T[2]*0.001

trans_matrix = np.array([T0 ,T1 , T2])

return trans_matrix

robot = YuMiRobot()
rhome = YuMiState([0, -130, 30, 0, 40, 0, -135])
robot.right.goto_state(rhome)
#robot.right.goto_state(YuMiState([23.4889, -102.875, 55.0657, 74.4949, -74.5303, 106.163, -54.7101]))

#robot.right.goto_state(YuMiState([23.4889, -102.875, 55.0657, 74.4949, -74.5303, 106.163, -54.7101]))
pose = robot.right.get_pose()

#pose.translation[0] += 0.05

print('pose',pose)
is_reachalbe = robot.right.is_pose_reachable(pose)
print(f'is pose reachable: {is_reachalbe}')
#rot_mat = quaternion_rotation_matrix([0.05747,-0.83779,-0.11153,-0.53139])
#trans_mat=trans_matrix([126.58,-150.87,131.17])
rot_mat = quaternion_rotation_matrix([0.05736,-0.83777,-0.11147,-0.53144])
trans_mat=trans_matrix([228.44,-150.86,131.19])
lin_pose_1 = RigidTransform(rot_mat, trans_mat,from_frame='tool',to_frame='world')
print ('lin1',lin_pose_1)

robot.right.goto_pose(pose)
#robot.right.goto_pose_delta((-0.05,0,0))

print('ok')

error:
/home/robot/sd1/venv/venv2/bin/python /home/robot/sd1/venv/test.py
WARNING:root:Failed to import geometry msgs in rigid_transformations.py.
WARNING:root:Failed to import ros dependencies in rigid_transforms.py
WARNING:root:autolab_core not installed as catkin package, RigidTransform ros methods will be unavailable
ERROR:root:Unable to load ROS! Will not be able to use client-side motion planner!
WARNING:root:rospy could not be imported, yumi over ros will be unavailable
pose Tra: [ 0.17658001 -0.15087 0.13117 ]
Rot: [[ 0.41098882 0.2482465 0.87718975]
[ 0.12717825 -0.96841394 0.21447643]
[ 0.90272581 0.02341205 -0.42957886]]
Qtn: [-0.05700006 0.83800084 0.11200011 0.53100053]
from tool to world
is pose reachable: False
lin1 Tra: [ 0.22844 -0.15086 0.13119]
Rot: [[ 0.41029748 0.24773924 0.87766114]
[ 0.12580565 -0.96856854 0.21458821]
[ 0.90323682 0.02237026 -0.42856271]]
Qtn: [-0.05736006 0.83776917 0.11147011 0.53144053]
from tool to world
Traceback (most recent call last):
File "/home/robot/sd1/venv/test.py", line 82, in
robot.right.goto_pose(pose)
File "/home/robot/.local/lib/python3.6/site-packages/yumipy/yumi_arm.py", line 559, in goto_pose
res = self._request(req, wait_for_res, timeout=self._motion_timeout)
File "/home/robot/.local/lib/python3.6/site-packages/yumipy/yumi_arm.py", line 314, in _request
raise YuMiControlException(req_packet, res)
yumipy.yumi_exceptions.YuMiControlException: Failed Request!
Req: _REQ_PACKET(req='1 176.6 -150.9 131.2 -0.057 0.838 0.112 0.531 #', timeout=8, return_res=True)
Res: _RAW_RES(mirror_code=1, res_code=0, message='Unreachable Pose')
Error in atexit._run_exitfuncs:
Traceback (most recent call last):
File "/usr/lib/python3.6/multiprocessing/popen_fork.py", line 28, in poll
pid, sts = os.waitpid(self.pid, flag)
KeyboardInterrupt

Process finished with exit code 1