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

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”