ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Wrong argument type when writing quaternion orientation to pose message

asked 2018-02-27 10:44:18 -0500

fitter gravatar image

updated 2018-03-05 07:23:30 -0500


I tried to just simply copy the quaternion values and hardcode them as an orientation coordinates. Results are the same, so maybe problem comes from another part of my grasp msg? I dumped the message which I'm trying to use to pick my object: link text

Hi, I'm trying to generate moveit_msgs/Grasp Message using Python API and I'm getting ROSSerializationException in place where calculated quaternion values are assigned into geometry_msgs/Pose Message/orientation.

Piece of code where I'm making Grasp message and trying to get quaternion from euler angles:

p = PickPlaceInterface("arm", "gripper", verbose=True)
g = Grasp() = "test"
gp = PoseStamped()
gp.header.frame_id = "base_link"
gp.pose.position.x = grasps[0].surface.x
gp.pose.position.y = grasps[0].surface.y
gp.pose.position.z = grasps[0].surface.z

quat = quaternion_from_euler(grasps[0].approach.x, grasps[0].approach.y, grasps[0].approach.z)

print ("Quat type: " + str(type(quat[0])))
print ("Pose orient type: " + str(type(gp.pose.orientation.x)))

gp.pose.orientation.x = float(quat[0]) 
gp.pose.orientation.y = float(quat[1])
gp.pose.orientation.z = float(quat[2])
gp.pose.orientation.w = float(quat[3])
g.grasp_pose = gp

g.pre_grasp_approach.direction.header.frame_id = "base_link"
g.pre_grasp_approach.direction.vector.x = 1.0 
g.pre_grasp_approach.direction.vector.y = 0.0 
g.pre_grasp_approach.direction.vector.z = 0.0 
g.pre_grasp_approach.min_distance = 0.001
g.pre_grasp_approach.desired_distance = 0.1 

g.pre_grasp_posture.header.frame_id = "base_link"
g.pre_grasp_posture.joint_names = ["gripper_axis"]
pos = JointTrajectoryPoint()

g.grasp_posture.header.frame_id = "base_link"
g.grasp_posture.joint_names = ["gripper_axis"]
pos = JointTrajectoryPoint()

g.allowed_touch_objects = []
g.max_contact_force = 0
g.grasp_quality = grasps[0].score

p.pickup("obj", [g, ], support_name="supp")

grasps[0].approach is a geometry_msgs/Vector3 message.

grasps is a list of GraspConfig.msg which is defined here: link text

I'm getting this exception:

 Traceback (most recent call last):   File "", line 83, in <module>
    err = p.pickup("obj", [g, ], support_name="supp")   File "/opt/ros/kinetic/lib/python2.7/dist-packages/moveit_python/", line 154, in pickup
    self._pick_action.send_goal(g)   File "/opt/ros/kinetic/lib/python2.7/dist-packages/actionlib/", line 92, in send_goal = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback)   File "/opt/ros/kinetic/lib/python2.7/dist-packages/actionlib/", line 553, in send_goal
    return self.manager.init_goal(goal, transition_cb, feedback_cb)   File "/opt/ros/kinetic/lib/python2.7/dist-packages/actionlib/", line 466, in init_goal
    self.send_goal_fn(action_goal)   File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/", line 886, in publish
    raise ROSSerializationException(str(e)) rospy.exceptions.ROSSerializationException: <class 'struct.error'>: 'required argument is not a float' when writing 'x: 0.008194649141243238 y:
0.1413781390046223 z: 0.44856469150891914 w: 0.8824595101581431'

Looks simple, I'm just messing with types. But when I checked them with:

print ("Quat type: " + str(type(float(quat[0]))))
print ("Pose orient type: " + str(type ...
edit retag flag offensive close merge delete


What is type(quat[0])) ?

NEngelhard gravatar image NEngelhard  ( 2018-02-28 01:48:00 -0500 )edit


fitter gravatar image fitter  ( 2018-02-28 02:39:03 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2018-03-05 07:20:18 -0500

fitter gravatar image

updated 2018-03-05 07:31:13 -0500

Ok, I figured out whats wrong. The problem is trivial, but because of the exception handling it was quite complicated to figure out whats wrong there.

Orientation values are completely OK, exception rises when grasp_quality value is serialized. I'm returning std_msgs/Float32 as a grasp_quality value. Fix is very easy:

g.grasp_quality = grasps[0].score #WRONG! returns std_msgs/Float32
g.grasp_quality = grasps[0] #returns float

Now interesting part. Why exception returns orientation values? Lets look how that data is serialized in (autogenerated code :/):

_v14 = _v12.orientation
_x = _v14 
buff.write(_get_struct_4d().pack(_x.x, _x.y, _x.z, _x.w))
buff.write(_get_struct_d().pack(val1.grasp_quality) #boom, error

#and part where exception is handled
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self)))))

Simply error printing uses _x to provide information where exception occured, but before writing grasp_quality value to buff _x is not updated, so it still contains orientation values, and those values are printed...

For me that looks like a bug, IMO _x should be updated before every buff.write(...).

edit flag offensive delete link more

Question Tools



Asked: 2018-02-27 10:44:18 -0500

Seen: 575 times

Last updated: Mar 05 '18