TypeError: float() argument must be a string or a number
Hello, I follow this repo and want to combine it to my robot.
But when I try to get the rotation and convert it to Quaternion. I encounter the error below:
cyc@cyc-X550JX:~$ rosrun gpd test_grasp.py
[INFO] [1558753204.760210]: Start to get grasp data...
[INFO] [1558753209.430311]: Received 1 grasps.
[x: 0.527165424825
y: 0.842580358548
z: 0.110249509106
x: 0.849633644563
y: -0.524891291709
z: -0.0511057914036
x: 0.0148082712001
y: 0.120612898469
z: -0.992589161651]
Traceback (most recent call last):
File "/home/cyc/catkin_ws/src/gpd/src/test_grasp.py", line 74, in <module>
qua_R = tf.transformations.quaternion_from_matrix(R)
File "/opt/ros/kinetic/lib/python2.7/dist-packages/tf/transformations.py", line 1206, in quaternion_from_matrix
M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4]
**TypeError: float() argument must be a string or a number**
Here is my code and the GraspConfig information:
#!/usr/bin/env python
import rospy
import sys
import tf
import numpy as np
from gpd.msg import GraspConfigList
from tf.transformations import quaternion_from_matrix
from geometry_msgs.msg import Pose
from geometry_msgs.msg import PoseStamped
grasps = []
def callback(msg):
global grasps
grasps = msg.grasps
# Create a ROS node.
rospy.init_node('get_grasps')
rospy.loginfo('Start to get grasp data...')
# Subscribe to the ROS topic that contains the grasps.
sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, callback)
# Wait for grasps to arrive.
rospy.sleep(1)
while not rospy.is_shutdown():
if len(grasps) > 0:
top_grasp = grasps[0] # grasps are sorted in descending order by score.
rospy.loginfo('Received %d grasps.', len(grasps))
R = (top_grasp.approach, top_grasp.binormal, top_grasp.axis)
R = np.asarray(R)
print(R)
qua_R = tf.transformations.quaternion_from_matrix(R)
print qua_R
break
rospy.spin()
Hope you guys can help me, thanks a lot!
The quaternion_from_matrix method is expecting a 3x3 rotation matrix, you appear to be passing it something very different here.
I copy the partial information from GraspConfig.msg :
So, I think the
R = (top_grasp.approach, top_grasp.binormal, top_grasp.axis)
should be a 3x3 roatation matrix. Does it have any mistakes in this ? Thanks for your reply !Okay I think I can see the problem.
quaternion_from_matrix
is expecting a 3x3 numpy array. But looking at the value printed out you're passing it something else. I think the three values approach, binormal, and axis are dictionaries not lists so you'll have to extract each element individually.Actually, I created a 3x3 Numpy matrix to storage the vector of three data today. But
quaternion_from_matrix
occur the error about matrix index is out of bound, although I usedmatrix.shape
to check before, it still not working. I tried convert to Euler first, then convert the output to Quaternion. Finally, it works. Thanks for your patience.