Forums Niryo One Troubleshooting Strange error when trying to get inverse kinematics using get_ik.py

Viewing 2 posts - 1 through 2 (of 2 total)
  • Author
    Posts
  • mgiglia
    Participant
    Post count: 3

    Hi All,

    I’m trying to use inverse kinematics to get joint angles for desired end effector positions to do some tasks as well as some flexibilitiy in code to allow for linear trajectories by publishing joints to the joint trajectory topic using the “JointMode” class.

    After much reading through documentation and help from Edouard I am able to finally publish to the joint topic successfully. Now onto the next step, trying to use the inverse kinematics service from ROS. I have downloaded the get_ik.py and imported it into my python script (I will paste my scrpit and the get_ik.py file below). It took me a while to get the message properties correct but I was able to send the ik service my message with a response. Unfortunately the response comes back mostly empty as there is still a “SerializationError” being thrown inside the get_ik.py file. I’ve gone through the stack and found the line where the error is being thrown (line 1009 in “_GetPositionIK.py”) but I cannot find out why it is being thrown and/or how to fix this.

    To prevent this post from being excessively long, here is the get_ik.py github link:
    https://github.com/uts-magic-lab/moveit_python_tools/blob/master/src/moveit_python_tools/get_ik.py

    Im curious if I’ve named my group or frame_id incorrectly, and the values I’m inputting are not in the enumeration table in ROS or something of the like but I can’t tell which value exactly is unable to be converted to an integer so I’m at a loss.
    ___________________________________________________________________________
    Terminal Output:

    — Start
    [INFO] [1551379274.003406]: Initalizing GetIK…
    [INFO] [1551379274.005399]: Computing IKs for group: niryo_one
    [INFO] [1551379274.007349]: With IK timeout: 5.0
    [INFO] [1551379274.009532]: And IK attempts: 5
    [INFO] [1551379274.012575]: Setting avoid collisions to: False
    [INFO] [1551379274.014956]: Waiting for /compute_ik service…
    [INFO] [1551379274.026009]: Connected!
    Calibration finished !
    Move robot to a position, then press any key
    Move robot to a second position, then press any key
    Traceback (most recent call last):
    File “./trajectory_test.py”, line 123, in <module>
    print(ik.get_ik(pose_stamped))
    File “/home/niryo/get_ik.py”, line 77, in get_ik
    resp = self.ik_srv.call(req)
    File “/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py”, line 512, in call
    transport.send_message(request, self.seq)
    File “/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_base.py”, line 665, in send_message
    serialize_message(self.write_buff, seq, msg)
    File “/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/msg.py”, line 152, in serialize_message
    msg.serialize(b)
    File “/opt/ros/kinetic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetPositionIK.py”, line 1009, in serialize
    except struct.error as se: self._check_types(struct.error(“%s: ‘%s’ when writing ‘%s'” % (type(se), str(se), str(locals().get(‘_x’, self)))))
    File “/opt/ros/kinetic/lib/python2.7/dist-packages/genpy/message.py”, line 336, in _check_types
    raise SerializationError(str(exc))
    genpy.message.SerializationError: <class ‘struct.error’>: ‘cannot convert argument to integer’ when writing ‘ik_request:
    group_name: “niryo_one”
    robot_state:
    joint_state:
    header:
    seq: 0
    stamp:
    secs: 0
    nsecs: 0
    frame_id: ”
    name: []
    position: []
    velocity: []
    effort: []
    multi_dof_joint_state:
    header:
    seq: 0
    stamp:
    secs: 0
    nsecs: 0
    frame_id: ”
    joint_names: []
    transforms: []
    twist: []
    wrench: []
    attached_collision_objects: []
    is_diff: False
    constraints:
    name: ”
    joint_constraints: []
    position_constraints: []
    orientation_constraints: []
    visibility_constraints: []
    avoid_collisions: False
    ik_link_name: ”
    pose_stamped:
    header:
    seq: 0
    stamp:
    secs: 1551379295031922101
    nsecs: 0
    frame_id: “joint0”
    pose:
    position:
    x: 0.0718291580352
    y: -0.00151157116934
    z: 0.164982453382
    orientation:
    x: 0.780808956892
    y: 0.00252309988441
    z: 0.624754538541
    w: -0.00357398609935
    ik_link_names: []
    pose_stamped_vector: []
    timeout:
    secs: 5
    nsecs: 0
    attempts: 5’

    ____________________________________________________________________________

    Python script:

    #!/usr/bin/env python

    # Niryo One libraries
    from niryo_one_python_api.niryo_one_api import *
    from joystick_interface import JointMode

    # Python libraies
    import sys
    import copy
    import rospy
    import time
    import math
    import numpy as np
    import pyquaternion

    # Ros messages to import
    from std_msgs.msg import Duration, Header
    from moveit_msgs.msg import PositionIKRequest, RobotState
    from geometry_msgs.msg import PoseStamped, Pose
    from get_ik import GetIK

    # Functions for simplicity
    def move_by_pose(n, pose):
    n.move_pose(pose.position.x, pose.position.y, pose.position.z, pose.rpy.roll, pose.rpy.pitch, pose.rpy.yaw)

    def euler_to_quaternion(roll, pitch, yaw):
    # Z rotation matrix (yaw)
    cy = math.cos(yaw)
    sy = math.sin(yaw)
    Rz = np.array([[math.cos(yaw), -1.0*math.sin(yaw), 0], [math.sin(yaw), math.cos(yaw), 0], [0, 0, 1]])
    # Yrotation matrix (pitch)
    cp = math.cos(pitch)
    sp = math.sin(pitch)
    Ry = np.array([[math.cos(pitch), 0, math.sin(pitch)], [0, 1, 0], [-1.0*math.sin(pitch), 0, math.cos(pitch)]])
    # X rotation matrix (roll)
    cr = math.cos(roll)
    sr = math.sin(roll)
    Rx = np.array([[1, 0, 0], [0, math.cos(roll), -1.0*math.sin(roll)], [0, math.sin(roll), math.cos(roll)]])
    # Total rotation matrix
    Rtemp = np.matmul(Rz, Ry)
    Rtotal = np.matmul(Rtemp, Rx)
    # print(Rtotal)
    # print(Rtotal*np.transpose(Rtotal))
    # print(np.linalg.inv(Rtotal) – np.transpose(Rtotal))
    # Get quaternion
    quaternion = pyquaternion.Quaternion(matrix = Rtotal)
    # quaternion = pyquaternion.Quaternion(matrix = np.eye(3))
    return quaternion

    print “— Start”

    # init necessary nodes
    #rospy.init_node(‘niryo_one_python_example_api’)
    rospy.init_node(‘robot_commander’)
    # Create the NiryoOne object in case we want to use some niryo functions
    n = NiryoOne()
    #commander = NiryoOneCommanderNode()
    j = JointMode()

    # Get inverse kinematics object
    ik = GetIK(group = ‘niryo_one’, ik_timeout = 5.0, ik_attempts = 5, avoid_collisions = False)

    try:
    #Calibrate Robot first
    n.calibrate_auto()
    print “Calibration finished !”

    time.sleep(1)

    # Set Tool to gripper 1 ID
    n.change_tool(TOOL_GRIPPER_1_ID)

    # Activate learning mode
    n.activate_learning_mode(True)

    # Get a first position
    raw_input(‘Move robot to a position, then press any key’)
    j0 = n.get_joints()

    # Get a second position
    raw_input(‘Move robot to a second position, then press any key’)
    j1 = n.get_joints()
    p1 = n.get_arm_pose()

    # Shut off learning mode
    n.activate_learning_mode(False)

    # Create PoseStamped message for ik request
    pose_stamped = PoseStamped()

    pose_stamped.pose.position.x = float(p1.position.x)
    pose_stamped.pose.position.y = float(p1.position.y)
    pose_stamped.pose.position.z = float(p1.position.z)

    # Get quaternion from rpy and set msg values
    quaternion = euler_to_quaternion(p1.rpy.roll, p1.rpy.pitch, p1.rpy.yaw)
    pose_stamped.pose.orientation.x = float(quaternion[0])
    pose_stamped.pose.orientation.y = float(quaternion[1])
    pose_stamped.pose.orientation.z = float(quaternion[2])
    pose_stamped.pose.orientation.w = float(quaternion[3])
    pose_stamped.header.frame_id = “joint0”
    pose_stamped.header.stamp.secs = rospy.Time.now()
    pose_stamped.header.seq = int(0)

    # Create IK Request message
    ik_msg1 = PositionIKRequest()
    ik_msg1.group_name = ‘niryo_one’
    ik_msg1.robot_state = RobotState()
    ik_msg1.pose_stamped = pose_stamped

    # Try and get inverse kinematics
    print(ik.get_ik(pose_stamped))

    # Amount of time to take to get from 1 point to another
    duration = 3 # seconds

    # Use JointMode class from niryo to publish trajectory
    j.enable()
    j.publish_joint_trajectory(j0, duration)
    time.sleep(3)
    j.publish_joint_trajectory(j1, duration)
    print(‘Moved successfully’)

    except NiryoOneException as e:
    print e
    # Handle errors here

    print “— End”

    mgiglia
    Participant
    Post count: 3

    Guys,

    I’m sorry for anyone who has read through this, I figured out a couple of errors by simple commenting out some lines of code and recommenting them. I really pulled the trigger too quick on the forum post.

    My rospy.Time.now() for some reason was throwing the SerializationError. Then after I fixed that I was able to use the error codes from the ik response to find that my group_name was incorrect and now I get a successfull inverse kinematic solution.

    SORRY!

    Best,
    Michael Giglia

Viewing 2 posts - 1 through 2 (of 2 total)

You must be logged in to reply to this topic.