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

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

    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: 4

    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

    michaelS
    Participant
    Post count: 4

    Glad to hear you found a solution! Would you please share how you set this up and any tutorials useful for someone trying to recreate your work? I’d really like to get into IK with my robot arm, but haven’t learned much about ROS yet to even know where to start. Thanks!

    mgiglia
    Participant
    Post count: 4

    Hi michaelS and others,

    So here is my solution, I will also post the code at the bottom, though it is quite messy.

    I found online, a get_ik.py service for ROS on github. (https://github.com/uts-magic-lab/moveit_python_tools/blob/master/src/moveit_python_tools/get_ik.py). Add this file to the directory the python script is in that way you can import it into the python script.

    Here are the necessary messages I’ve found I needed to get this all running:
    # 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

    Secondly, I use a nice little quaternion class I have also found on Github called pyquaternion. This is a great class and has a lot of methods which are very useful and can be installed using pip. Thank you Kieran for this great class if you ever happen to see this post.
    (https://github.com/KieranWynn/pyquaternion)
    This class is to convert the Euler roll, pitch, yaw values into a quaternion, which the inverse kinematics service requires. I do this by creating a rotation matrix from the 3 orientation angles, and then use the quaternion constructor which accepts a 3×3 rotation matrix as a construction parameter.

    Okay, so my script here is used to write a letter on a piece of paper by teaching the Niryo robot multiple positions. These positions are then normalized to force the z-position of all points to be the same (that way the pen stays in a plane) and the pitch values are all forced to 0 to make sure the pen is as vertical as possible. I do this with some functions defined at the top of the script.

    After this I linearly interpolated between each point to get multiple points, to hopefully keep the trajectory as linear as possible to prevent the pen from being stabbed into the paper. I found that the quintic spline movements from the niryo_one_api (and the JointMode movements) caused the pen to be forced into the paper causing it to rotate and ultimately not properly write the letter. So interpolating many points between longer movements has reduced this forcing almost entirely, I interpolate about 10 points.

    After interpolation I run the inverse kinematics on the list of poses (which were first turned into the proper messages that the ik service accepts) and save the new list of joint positions that the JointMode class will accept to actually move the robot.

    And finally I use the “move_through_joint_positions” function to publish the joint positions to the JointMode class at a desired duration between points. (Note that this is the time between each interpolated point).

    So hopefully my hacky solution will help someone out, and I’d be happy to hear of cleaner ways to write this script. Sorry I was so late on this, I did not have time at work in the past week or two to pull the code off the robot. There is one bug, and that is when you run through the trajectory once, when you try to run through the trajectory again, it starts at the second learned point, not the first. I believe this is an error in one of my for loops, but haven’t spent the time to fix this as this was only a test script to see if I could get some nice control capabilities for the students in the industrial robotics class. Oh also, the script prompts you through the learning phase and asks you if you wish to run through the learned points.

    CODE:_____________________________________________________________________________________________________

    #!/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).unit

    return quaternion
    def normalize_poses(poses):
    # Make all poses have same z position, and 0 pitch value
    poses[0].rpy.pitch = 0
    z_norm = poses[0].position.z

    for i in range(1, len(poses)):
    poses[i].position.z = z_norm
    poses[i].rpy.pitch = 0
    return poses

    def make_ik_msg(pose):
    # Create PoseStamped message for ik request
    ps1 = PoseStamped()

    ps1.pose.position.x = float(pose.position.x)
    ps1.pose.position.y = float(pose.position.y)
    ps1.pose.position.z = float(pose.position.z)

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

    return ps1

    def interpolate_poses(poses, n):
    # Interpolate positions to get list of interpolated positions
    output_poses = []

    for j in range(0, len(poses)-1):
    dxj = (poses[j+1].position.x – poses[j].position.x) / n
    dyj = (poses[j+1].position.y – poses[j].position.y) / n
    dzj = (poses[j+1].position.z – poses[j].position.z) / n
    drj = (poses[j+1].rpy.roll – poses[j].rpy.roll) / n
    dpj = (poses[j+1].rpy.pitch – poses[j].rpy.pitch) / n
    dyawj = (poses[j+1].rpy.yaw – poses[j].rpy.yaw) / n
    print(‘dx’, dxj, ‘dy’, dyj, ‘dz’, dzj)

    temp_pose = poses[j]
    output_poses.append(copy.deepcopy(temp_pose))

    for i in range(1, n):
    temp_pose.position.x = poses[j].position.x + dxj
    temp_pose.position.y = poses[j].position.y + dyj
    temp_pose.position.z = poses[j].position.z + dzj
    temp_pose.rpy.roll = poses[j].rpy.roll + drj
    temp_pose.rpy.pitch = poses[j].rpy.pitch + dpj
    temp_pose.rpy.yaw = poses[j].rpy.yaw + dyawj
    print(‘temp_pose for’, i, ‘data’, temp_pose.position)
    # Append new temp pose to list of output poses
    output_poses.append(copy.deepcopy(temp_pose))
    # Append the last learned position onto the list
    output_poses.append(copy.deepcopy(poses[len(poses) – 1]))
    print(‘Finished interpolation’)
    print(output_poses)
    return output_poses

    def run_ik_on_poses(poses, ik):
    # Convert poses using get_ik to get list in joint space
    output_joints = []

    for i in range(0, len(poses)):
    print(‘Running ik on joint[‘, i, ‘]’)
    msg = make_ik_msg(poses[i])
    response = ik.get_ik(msg)
    output_joints.append(copy.deepcopy(response.solution.joint_state.position))
    print(response)
    print(‘Finished ik’)
    return output_joints # return the list of joints

    def move_through_joint_positions(joints, duration, j):
    # Move through all joint positions in list
    j.enable()
    for i in range(0, len(joints)):
    print(joints[i])
    j.publish_joint_trajectory(joints[i], duration)
    time.sleep(duration)
    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 = ‘arm’, 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)

    # List of poses to add to
    poses = []
    # Loop to add points to movement command
    while(True):
    try:
    n.activate_learning_mode(True)
    input = raw_input(“Would you like to add a point? (y/n)”)

    if(input == ‘y’):
    temp_pose = n.get_arm_pose()
    poses.append(temp_pose)
    print(‘Added pose’, temp_pose)
    elif(input == ‘n’):
    # Move on to interpolating and ik conversion
    break
    else:
    print(‘Value you input is not valid’)
    pass

    except Exception as e:
    print(‘Some error, trying again’)
    print e
    pass

    while(True):
    try:
    input = raw_input(‘Enter r to run command, q to quit’)

    if(input == ‘r’):

    # Normalize poses so z val and pitch are the same throughout movement
    normalized_poses = normalize_poses(poses)

    # Interpolate points
    interpolated_poses = interpolate_poses(normalized_poses, 5)

    # Convert poses to joints using ik
    joints = run_ik_on_poses(interpolated_poses, ik)

    # Deactivate learning mode so robot can move
    n.activate_learning_mode(False)

    # Move to beginning position
    j.enable()
    j.publish_joint_trajectory(joints[0], 2)
    time.sleep(3)

    # Open Gripper
    n.open_gripper(TOOL_GRIPPER_1_ID, 300)

    # Tell user to put pen in gripper
    raw_input(‘Put pen in gripper, press enter to close’)

    # Close gripper
    n.close_gripper(TOOL_GRIPPER_1_ID, 300)

    # Move robot through joint positions
    raw_input(‘Press enter to move through all learned positions’)
    move_through_joint_positions(joints, 0.25, j)
    pass
    elif(input == ‘q’):

    # Re-activate learning mode so you can move robot arm after script ends
    n.activate_learning_mode(True)
    print(‘Quitting’)
    break
    else:
    print(‘Invalid input, try again’)
    pass
    except Exception as e:
    print(‘Some error’)
    print e
    pass
    # duration = 5
    # for i in range(0, len(joints)):
    # j.publish_joint_trajectory(joints[i], duration)
    # time.sleep(duration)

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

    # 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()
    # print(‘pose1’, p1)

    # Shut off learning mode
    # n.activate_learning_mode(False)

    # Create pose staemped message for ik reuqest from pose value
    # ik_msg0 = make_ik_msg(p0)
    # ik_msg1 = make_ik_msg(p1)

    # Get inverse kinematics
    # response0 = ik.get_ik(ik_msg0)
    ## response1 = ik.get_ik(ik_msg1)
    # print(‘ik responses’)
    # print(response0)
    # print(response1)

    # ik_joints0 = response0.solution.joint_state.position
    # ik_joints1 = response1.solution.joint_state.position
    # 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(ik_joints0, duration)
    # time.sleep(6)
    # print(‘p0’, p0)
    # print(‘p0_new’, n.get_arm_pose())
    # j.publish_joint_trajectory(ik_joints1, duration)
    # time.sleep(6)
    # print(‘p1’, p1)
    # print(‘p1_new’, n.get_arm_pose())

    print(‘Moved successfully’)

    except NiryoOneException as e:
    print e
    # Handle errors here

    print “— End”

    michaelS
    Participant
    Post count: 4

    This is awesome progress, thank you so much for sharing. Will report back when I get to testing it myself.

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

You must be logged in to reply to this topic.