Forum Replies Created

Viewing 2 posts - 1 through 2 (of 2 total)
  • Author
    Posts
  • 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”

    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

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