def __init__(self, arm_definition):
for i, val in enumerate(arm_definition): # Fix axes
if arm_definition[i] == "z":
arm_definition[i] = "y"
elif arm_definition[i] == "y":
arm_definition[i] = "z"
self._arm = tinyik.Actuator(arm_definition)
def handle_ik(iksrvin):
arm = ik.Actuator([
"z", [0, 0, 0.15], "x", [0, 0, 2.0], "x", [0, 0, 1.0], "x", [0, 0, .5],
"z", [0, 0, 0.02], "x", [0, 0, 0.02]
])
arm.angles = iksrvin.joint_positions
arm.ee = iksrvin.actuator_pose
return IkResponse(arm.angles)
def handleIK(req):
arm = tik.Actuator([
'z', [0, 0, 0.15], 'x', [0, 0, 2], 'x', [0, 0, 1], 'x', [0, 0, 0.5],
'y', [0, 0, 0.2], 'z', [0, 0, 0.1]
])
arm.ee = ee
# return arm.angles
# angles = TC.ikTransform(req.links, req.ee)
return IKResponse(arm.angles)
def create_arm(self):
base_height = [0., 0., 3.25]
upper_arm_length = [0., 0., 4.75]
lower_arm_length = [0., 0., 5.0]
axis_of_rotation = ['z', 'y', 'y']
return tinyik.Actuator([
axis_of_rotation[0], base_height, axis_of_rotation[1],
upper_arm_length, axis_of_rotation[2], lower_arm_length
])
def handle_add_two_ints(req):
print("Returning")
arm = tinyik.Actuator([
req.Arm1Axis, [req.Arm1X, req.Arm1Y, req.Arm1Z], req.Arm2Axis,
[req.Arm2X, req.Arm2Y, req.Arm2Z], req.Arm3Axis,
[req.Arm3X, req.Arm3Y, req.Arm3Z], req.Arm4Axis,
[req.Arm4X, req.Arm4Y, req.Arm4Z]
])
arm.angles = np.deg2rad(
[req.Arm1Angle, req.Arm2Angle, req.Arm3Angle, req.Arm4Angle])
return fkResponse(arm.ee[0], arm.ee[1], arm.ee[2])
def toActuator(self):
if(not self.hasRevolute):
print("revolute is not setted")
raise RevoluteError
structure=[]
self.toHomogeneus()
for i in range(len(self.revolute)):
structure.append(self.revolute[i])
structure.append(list(self.hChain.elements[i+1].carthesian()))
#print(structure)
actuator=tinyik.Actuator(structure)
return actuator
def Handle_IK(req):
desired_pose = req.desired_pose
arm = tk.Actuator([
'z', [0, 0, 0.15],
'x', [0, 0, 2.0],
'x', [0, 0, 1.0],
'x', [0, 0, 0.5],
'z', [0, 0, 0.1],
'y', [0, 0, 0.1],
])
arm.ee = [desired_pose[0], desired_pose[1], desired_pose[2]]
return ikResponse(arm.angles)
def __init__(self,app,*arg,**kw):
Plan.__init__(self,app,*arg,**kw)
self.app = app
self.steps = []
#Make another arm with the same orientation and arm lenght segments as
#what is defined in armSpec. This is what is used for inverse kinematics.
self.moveArm = tinyik.Actuator(['z',[5.,0.,0.],'y',[5.,0.,0.],'y',[5.,0.,0.]])
self.moveArm.angles = self.app.armSpec[-1,:]
self.pos = [] #Goal position for move. Will either be grid point or square corner.
self.calibrated = False
self.CalDone = False
self.square = False
self.currentPos = []
self.curr = 0
def handle_add_two_ints(req):
temp = []
k = 0
#print(req.joint_axis)
arm = tinyik.Actuator([
"z", [0, 0, 0.15], "x", [0, 0, 2.0], "x", [0, 0, 1.0], "x", [0, 0, .5],
"z", [0, 0, 0.02], "x", [0, 0, 0.02]
])
arm.angles = req.joint_values
arm.ee = req.target_values
print(req.joint_values)
print(arm.angles)
print(req.target_values)
return ikResponse(arm.angles)
def __init__(self,
name,
board,
servoNum,
inverse=[0, 0, 0],
offset=[0, 0, 0]):
self.name = name
self.s1 = board.servo[servoNum]
self.s2 = board.servo[servoNum + 1]
self.s3 = board.servo[servoNum + 2]
self.angles = [90, 90, 90]
self.homeAngles = [0, -5, -90]
self.homePoint = [11., 0., 13.]
self.inverse = inverse
self.offset = offset
self.actuator = tinyik.Actuator(
['z', [4., .0, .0], 'y', [7.5, .0, .0], 'y', [12.5, 0., .0]])
self.actuator.angles = np.deg2rad(self.homeAngles)
def handle_inverseKinematics(req):
arm = ik.Actuator([
"z", [0, 0, 0.15], #base arm1
"x", [0, 0, 2.0], #arm2
"x", [0, 0, 1.0], #arm3
"x", [0, 0, 0.5], #arm4
"z", [0, 0, 0.1], #palm
"x", [0, 0, 0.1], #palm_jnt
"z", [0, 0, 0.3], #fingers
"z", [0, 0, 0.2] #tip
])
arm.ee = req.actuator_pose
print(arm.angles)
return IKResponse(arm.angles)
def inverse_kinematics(end_effect=[0, 0]):
# arm = tinyik.Actuator(['x', [0., 0., 4.], 'x', [0., 0., 2.], 'x', [0., 0., 2.]])
arm2 = tinyik.Actuator(['y', [0., 0., 1.], 'y', [0., 0., 1.]])
arm2.ee = end_effect
angle1, angle2 = arm2.angles
angle3 = 1.5708 - (angle1 + angle2)
# if angle3 > 3.14:
# angle3 = angle3 - 3.14
# elif angle3 < -3.14:
# angle3 = angle3 + 3.14
# else:
# angle3 = angle3
angle1 = round(angle1, 4)
angle2 = round(angle2, 4)
angle3 = round(angle3, 4)
print(angle1, angle2, angle3)
return [angle1, angle2, angle3]
def handle_ik(req):
arm = ik.Actuator([
"z",
[0, 0, 0.15], #base arm1
"x",
[0, 0, 2.0], #arm2
"x",
[0, 0, 1.0], #arm3
"x",
[0, 0, 0.5], #arm4
"z",
[0, 0, 0.1], #palm
"y",
[0, 0, 0.1] #palm_jnt
# "z", [0, 0, 0.4]
])
arm.ee = req.actuator_pose
print(arm.angles)
return IKResponse(arm.angles)
def IKRequestHandler(req):
actuatorPose = req.actuatorPose
arm = ik.Actuator([
'z',
[0, 0, 0.15],
'x',
[0, 0, 2.0],
'x',
[0, 0, 1.0],
'x',
[0, 0, 0.5],
'z',
[0, 0, 0.1],
'y',
[0, 0, 0.1],
])
arm.ee = [actuatorPose[0], actuatorPose[1], actuatorPose[2]]
print(arm.angles)
print("Returning: ", arm.angles)
return IKResponse(arm.angles)
t[i] = time.time() - now
print("Average Time for {} Random Solutions: {}".format(
cycle_count, np.mean(t)))
np.random.seed(seed=123)
arm = tinyik.Actuator([
'z',
[0., 0., 1.],
'x',
[1., 0., 0.],
'y',
[0., 1., 0.],
'z',
[0., 0., 1.],
'x',
[1., 0., 0.],
'y',
[0., 1., 0.],
'z',
[0., 0., 1.],
],
optimizer=ScipyOptimizer())
print("Angles: {} -- EE: {}".format(arm.angles, arm.ee))
arm.angles = [np.pi / 2, 0, 0, 0, 0, 0, 0]
print("Angles: {} -- EE: {}".format(arm.angles, arm.ee))
arm.angles = [0, 0, 0, 0, 0, np.pi / 2, 0]
print("Angles: {} -- EE: {}".format(arm.angles, arm.ee))
des_ee = np.hstack((pyquaternion.Quaternion.random().elements, [2, 0, 0]))
return [a1, a2, a3]
def get_position(a1, a2, a3):
arm.angles = [a1, a2, a3]
x = arm.ee[0] + l0
y = arm.ee[1]
return [x, y]
####Dimensions of average male human being:
# l0 = 300 // l1 = 302 // l2 = 269 // l3 = 100
# joint angle range: a1->(-40, 90) // a2 -> (-10, 140) // a3 -> (-50, 60)
[l0, l1, l2, l3] = [0.3, 0.302, 0.269, 0.100]
arm = ti.Actuator(["z", [l1, 0., 0.], "z", [l2, 0., 0.], "z", [l3, 0., 0.]])
# initial position
[as1, as2, as3] = np.deg2rad([76.1, 90, 9.4])
arm.angles = [as1, as2, as3]
[xs, ys] = [arm.ee[0] + l0, arm.ee[1]]
print('initial position:', [xs, ys])
# moment of inertia
mass = 1
J1 = (mass * (l1 * m.cos(as1) + (l2 * m.cos(as1 + as2))**2 +
(l1 * m.sin(as1) + l2 * m.sin(as1 + as2))**2))
J2 = (mass * l2**2)
J3 = (mass * l3**2)
# target position
ports = pypot.dynamixel.get_available_ports()
print('available ports:', ports)
if not ports:
raise IOError('No port available.')
port = ports[0]
print('Using the first on the list', port)
dxl_io = pypot.dynamixel.DxlIO('/dev/ttyUSB0')
print('Connected!')
found_ids = dxl_io.scan([2, 3, 4, 5, 6])
print('Found ids:', found_ids)
arm = tinyik.Actuator([[0., -0.045, 0.], 'y', [0., -0.04, 0.], 'x',
[0., -0.123, 0.], 'x', [0., -0.103, 0.]])
#arm.angles = [np.pi / 6, np.pi / 3] # or np.deg2rad([30, 60])
arm.ee = [0.0, -0.3, -0.1]
arm_deg = np.round(np.rad2deg(arm.angles))
#dxl_io.set_goal_position({found_ids[0]: 0,found_ids[2]: 0,found_ids[1]: 0})
#dxl_io.set_moving_speed(dict(zip((3,), itertools.repeat(120))))
#dxl_io.set_goal_position(dict(zip((3,), itertools.repeat(0))))
#dxl_io.set_moving_speed(dict(zip((5,), itertools.repeat(120))))
#dxl_io.set_goal_position(dict(zip((5,), itertools.repeat(0))))
#dxl_io.set_moving_speed(dict(zip((4,), itertools.repeat(120))))
#dxl_io.set_goal_position(dict(zip((4,), itertools.repeat(0))))
#dxl_io.set_moving_speed(dict(zip((6,), itertools.repeat(150))))
#dxl_io.set_goal_position(dict(zip((6,), itertools.repeat(80))))
dxl_io.set_moving_speed(dict(zip((2, ), itertools.repeat(-500))))
time.sleep(1)
def trajectory_produce(h0, a, b, period, amp, phase):
"""
This is used to produce a trajectory function for leg robot
:param h0: the height of the hip
:param a: the step size along the z direction
:param b: the time step along the x direction
:param period: time period of one gait
:param amp: Amp is the upper body swing amplitude
:return: interpolation function
"""
# ============================================
# define the robot kinematic model
thigh_length = 0.353 # length of the thigh, the static parameter
shank_length = 0.350 # length of the shank, the static parameter
# suppose leg1 is the right leg, supporting leg
# suppose leg2 is the left leg, swinging leg
leg1 = ik.Actuator(
['y', [0, 0, -thigh_length], 'y', [0, 0, -shank_length]])
leg1.angles = [-0.01, 0.01] # init the configuration of the leg1
leg2 = ik.Actuator(
['y', [0, 0, -thigh_length], 'y', [0, 0, -shank_length]])
leg2.angles = [-0.01, 0.01] # init the configuration of the leg2
sample_num = 10 # number of the sampling points in half cycle
# the first half cycle
leg1_aim_x = np.linspace(0, -b, sample_num)
leg1_aim_y = np.zeros(sample_num)
leg1_aim_z = np.ones(sample_num) * -h0
leg1_aim = np.stack((leg1_aim_x, leg1_aim_y, leg1_aim_z), axis=-1)
leg1_angle = np.zeros((sample_num, 2))
theta_temp = np.linspace(0, pi, sample_num)
curve_x = a * np.sin(theta_temp)
curve_y = -b * np.cos(theta_temp)
leg2_aim_x = leg1_aim_x + curve_y
leg2_aim_y = leg1_aim_y
leg2_aim_z = leg1_aim_z + curve_x
leg2_aim = np.stack((leg2_aim_x, leg2_aim_y, leg2_aim_z), axis=-1)
leg2_angle = np.zeros((sample_num, 2))
for i in range(sample_num):
leg1.ee = leg1_aim[i, :]
leg1_angle[i, :] = leg1.angles
leg2.ee = leg2_aim[i, :]
leg2_angle[i, :] = leg2.angles
leg1_angle = np.stack((leg1_angle[:, 0], leg1_angle[:, 1]), axis=-1)
leg2_angle = np.stack((leg2_angle[:, 0], leg2_angle[:, 1]), axis=-1)
leg1_hip = leg1_angle[:, 0]
leg1_knee = leg1_angle[:, 1]
leg1_ankle = -(leg1_angle[:, 0] + leg1_angle[:, 1])
leg2_hip = leg2_angle[:, 0]
leg2_knee = leg2_angle[:, 1]
leg2_ankle = -(leg2_angle[:, 0] + leg2_angle[:, 1])
angle_control = np.stack(
(leg1_hip, leg1_knee, leg1_ankle, leg2_hip, leg2_knee, leg2_ankle),
axis=-1)
# the second half cycle
angle_control_2 = np.hstack((angle_control[:, 3:6], angle_control[:, 0:3]))
# total period
angle_control = np.vstack((angle_control, angle_control_2))
# np.savetxt('test1.txt', angle_control)
# mapping to the real robot configuration
# angle_control[:, 0:3] = -angle_control[:, 0:3]
# angle_control[:, 0] = -angle_control[:, 0]
# angle_control[:, 3] = -angle_control[:, 3]
# angle_control[:, 1:4] = -angle_control[:, 1:4]
angle_control[:, 0] = -angle_control[:, 0]
angle_control[:, 4] = -angle_control[:, 4]
angle_control[:, 5] = -angle_control[:, 5]
# angle_control[:, 2] = -angle_control[:, 2]
# temp = np.copy(angle_control[:, 0:3])
# angle_control[:, 0:3] = np.copy(angle_control[:, 3:6])
# angle_control[:, 3:6] = np.copy(temp)
# angle_control = angle_control
# custom_cycler = (cycler(color=['r', 'r', 'r', 'g', 'g', 'g']) +
# cycler(lw=[1, 2, 3, 1, 2, 3]) +
# cycler(linestyle=['-', '--', ':', '-', '--', ':']))
# plt.rc('axes', prop_cycle=custom_cycler)
# fig, (ax) = plt.subplots(nrows=1)
# # for i in range(6):
# # ax.plot(np.transpose(self.angle_control[:, i]))
# ax.plot(angle_control / pi * 180)
# # ax.set_prop_cycle(custom_cycler)
# plt.show()
global tck_leg1_hip, tck_leg1_knee, tck_leg1_ankle, tck_leg2_hip, tck_leg2_knee, tck_leg2_ankle, tck_upper
# interpolation
time_array = np.linspace(0, period, sample_num * 2)
tck_leg1_hip = interpolate.splrep(time_array, angle_control[:, 0], s=0)
tck_leg1_knee = interpolate.splrep(time_array, angle_control[:, 1], s=0)
tck_leg1_ankle = interpolate.splrep(time_array, angle_control[:, 2], s=0)
tck_leg2_hip = interpolate.splrep(time_array, angle_control[:, 3], s=0)
tck_leg2_knee = interpolate.splrep(time_array, angle_control[:, 4], s=0)
tck_leg2_ankle = interpolate.splrep(time_array, angle_control[:, 5], s=0)
tck_upper = interpolate.splrep(
time_array, amp * np.sin(2 * pi * time_array / period + phase), s=0)
return None
def coord2Angle(self):
actuator = tinyik.Actuator(
['z', self.bodypart1coord, 'z', self.bodypart2coord])
actuator.ee = self.eepartcoord
return (np.rad2deg(actuator.angles))
#A few useful and miscellaneous functions
import cv2
import numpy
from math import sqrt
import tinyik
#Defining the arm model used by the tinyik library
# to compute the inverse kinematic
arm = tinyik.Actuator(
['y', [0., 0., 0.], 'x', [0., 1., 0.], 'x', [0., 1., 0.]])
arm.ee = [1, 1, 1]
#Takes an image as input and returns a list of possible circle found
def getCircles(image):
#Blur the image to ease the detection of circles
imageB = cv2.medianBlur(image, 5)
#Turn it into grayscale
imageG = cv2.cvtColor(imageB, cv2.COLOR_BGR2GRAY)
#Call the opencv library to find circles
circles = cv2.HoughCircles(imageG,
cv2.cv.CV_HOUGH_GRADIENT,
1,
100,
param1=17,
param2=8)
return circles
#Computes the euclidian distance between two points
# represented by their coordinates a & b in lists
ports = pypot.dynamixel.get_available_ports()
print('available ports:', ports)
if not ports:
raise IOError('No port available.')
port = ports[0]
print('Using the first on the list', port)
dxl_io = pypot.dynamixel.DxlIO('/dev/ttyUSB0')
print('Connected!')
motor_ids = dxl_io.scan([3,4,5])
print('Found ids:', motor_ids)
arm = tinyik.Actuator([[0.0, -0.055, -0.115],'y',[0., -0.046, 0.],'x', [0., -0.2, 0.], 'x', [0., -0.18, 0.05]])
#arm.angles = [np.pi / 6, np.pi / 3] # or np.deg2rad([30, 60])
#dxl_io.set_goal_position({found_ids[0]: 0,found_ids[2]: 0,found_ids[1]: 0})
dxl_io.set_moving_speed(dict(zip((3,), itertools.repeat(50))))
dxl_io.set_goal_position(dict(zip((3,), itertools.repeat(0))))
dxl_io.set_moving_speed(dict(zip((5,), itertools.repeat(50))))
dxl_io.set_goal_position(dict(zip((5,), itertools.repeat(-45))))
dxl_io.set_moving_speed(dict(zip((4,), itertools.repeat(50))))
dxl_io.set_goal_position(dict(zip((4,), itertools.repeat(-40))))
time.sleep(1)
arm_counter =0
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 4 13:56:37 2019
@author: abhijithneilabraham
"""
'''
FORWARD KINEMATICS FIRST
'''
import tinyik
arm = tinyik.Actuator(
['z', [1., 0., 0.], 'x', [0., 0., 2.], 'x', [0., 0., 1.]])
print('when joint angles are zero by default,angles=', arm.angles)
print('end effector position=', arm.ee)
import numpy as np
arm.angles = [
np.pi / 6, np.pi / 3, np.pi / 4
] #pi is 180 in degrees,so this line gives us 30 and 60 and 45 degree angles
print('after updating angles', arm.ee)
'''
NOW ,INVERSE KINEMATICS
I am trying to reverse the same end effector position to find the original 3 angles to prove this works
'''
arm.ee = [2.21501372, -1.8365163, 0.74118095]
print('The angles from inverse kinematics of 3 joint are',
np.round(np.rad2deg(arm.angles)))
import numpy as np
import pybullet as p
import pybullet_data as pd
import time
import math
import tinyik
import os
# actuator design for individual leg
leg = tinyik.Actuator([[-.49, .0, 1.9], 'z', [-.62, .0, .0], 'x',
[.0, -2.09, .0], 'x', [.0, -1.8, .0]])
def current_milli_time():
return round(time.time(), 6)
class cheetah_class:
position = np.array([0.0, 0.0, 0.0])
orientation = np.array([0.0, 0.0, 0.0])
joint_angles = np.array([
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0
])
joint_vels = np.array([
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0
])
joint_accs = np.array([
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0
import numpy as np
import tinyik
init = np.array([0.0, 0.0, 0.0])
link1 = np.array([0, 0.0, 10.0])
link2 = np.array([0, 0.0, 12.0])
link3 = np.array([0, 0.0, 9.0])
link4 = np.array([0, 0.0, 6.0])
link5 = np.array([0, 0.0, 12.0])
arm = tinyik.Actuator(
['z', link1, 'y', link2, 'y', link3, 'y', link4, 'z', link5])
print(
"\nSince the joint angles are zero by default, the end-effector position is at:"
)
print("arm.angles: ", arm.angles)
print("arm.ee: ", arm.ee)
#
# print("\nSets the joint angles to 30 and 60 degrees to calculate a new position of the end-effector")
# arm.angles = [np.pi / 6, np.pi / 3] # or np.deg2rad([30, 60])
# print("arm.ee: ", arm.ee)
#
print("\nSets a position of the end-effector to calculate the joint angles")
# arm.ee = [2 / np.sqrt(2), 2 / np.sqrt(2), 0.]
arm.ee = [0.5, 0.5, 0.0]
print("arm.angles: ", arm.angles)
print("rad2 degrees: ", np.round(np.rad2deg(arm.angles)))
# TODO: -1500 to 1500 into degrees
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def trajectory_produce(h0, a, b, period, amp, phase):
"""
This is used to produce a trajectory function for leg robot
:param h0: the height of the hip
:param a: the step size along the z direction
:param b: the time step along the x direction
:param period: time period of one gait
:param amp: Amp is the upper body swing amplitude
:return: interpolation function
"""
# ============================================
# define the robot kinematic model
thigh_length = 0.353 # length of the thigh, the static parameter
shank_length = 0.350 # length of the shank, the static parameter
# suppose leg1 is the right leg, supporting leg
# suppose leg2 is the left leg, swinging leg
leg1 = ik.Actuator(
['y', [0, 0, -thigh_length], 'y', [0, 0, -shank_length]])
leg1.angles = [-0.01, 0.01] # init the configuration of the leg1
leg2 = ik.Actuator(
['y', [0, 0, -thigh_length], 'y', [0, 0, -shank_length]])
leg2.angles = [-0.01, 0.01] # init the configuration of the leg2
sample_num = 10 # number of the sampling points in half cycle
# the first half cycle
leg1_aim_x = np.linspace(0, -b, sample_num)
leg1_aim_y = np.zeros(sample_num)
leg1_aim_z = np.ones(sample_num) * -h0
leg1_aim = np.stack((leg1_aim_x, leg1_aim_y, leg1_aim_z), axis=-1)
leg1_angle = np.zeros((sample_num, 2))
theta_temp = np.linspace(0, pi, sample_num)
curve_x = a * np.sin(theta_temp)
curve_y = -b * np.cos(theta_temp)
leg2_aim_x = leg1_aim_x + curve_y
leg2_aim_y = leg1_aim_y
leg2_aim_z = leg1_aim_z + curve_x
leg2_aim = np.stack((leg2_aim_x, leg2_aim_y, leg2_aim_z), axis=-1)
leg2_angle = np.zeros((sample_num, 2))
for i in range(sample_num):
leg1.ee = leg1_aim[i, :]
leg1_angle[i, :] = leg1.angles
leg2.ee = leg2_aim[i, :]
leg2_angle[i, :] = leg2.angles
leg1_angle = np.stack((leg1_angle[:, 0], leg1_angle[:, 1]), axis=-1)
leg2_angle = np.stack((leg2_angle[:, 0], leg2_angle[:, 1]), axis=-1)
leg1_hip = leg1_angle[:, 0]
leg1_knee = leg1_angle[:, 1]
leg1_ankle = -(leg1_angle[:, 0] + leg1_angle[:, 1])
leg2_hip = leg2_angle[:, 0]
leg2_knee = leg2_angle[:, 1]
leg2_ankle = -(leg2_angle[:, 0] + leg2_angle[:, 1])
angle_control = np.stack(
(leg1_hip, leg1_knee, leg1_ankle, leg2_hip, leg2_knee, leg2_ankle),
axis=-1)
# the second half cycle
angle_control_2 = np.hstack((angle_control[:, 3:6], angle_control[:, 0:3]))
# total period
angle_control = np.vstack((angle_control, angle_control_2))
# mapping to the real robot configuration
angle_control[:, 0:3] = -angle_control[:, 0:3]
angle_control[:, 0] = -angle_control[:, 0]
angle_control[:, 3] = -angle_control[:, 3]
temp = np.copy(angle_control[:, 0:3])
angle_control[:, 0:3] = np.copy(angle_control[:, 3:6])
angle_control[:, 3:6] = np.copy(temp)
angle_control = angle_control
global tck_leg1_hip, tck_leg1_knee, tck_leg1_ankle, tck_leg2_hip, tck_leg2_knee, tck_leg2_ankle, tck_upper
# interpolation
time_array = np.linspace(0, period, sample_num * 2)
tck_leg1_hip = interpolate.splrep(time_array, angle_control[:, 0], s=0)
tck_leg1_knee = interpolate.splrep(time_array, angle_control[:, 1], s=0)
tck_leg1_ankle = interpolate.splrep(time_array, angle_control[:, 2], s=0)
tck_leg2_hip = interpolate.splrep(time_array, angle_control[:, 3], s=0)
tck_leg2_knee = interpolate.splrep(time_array, angle_control[:, 4], s=0)
tck_leg2_ankle = interpolate.splrep(time_array, angle_control[:, 5], s=0)
tck_upper = interpolate.splrep(
time_array, amp * np.sin(2 * pi * time_array / period + phase), s=0)
return None
def buildFrontLeg():
leg = tinyik.Actuator([[.0000000001, .0, .0], 'z', [.0, -1.76, .0], 'x', [.0, -5.38, .0], 'z', [.0, -8.11, .0]])
# leg.angles = np.deg2rad([0,0,0])
# tinyik.visualize(leg)
return leg
import numpy as np
import tinyik
arm = tinyik.Actuator(['z', [1., 0., 0.], 'z', [1., 0., 0.]])
#Since the joint angles are zero by default, the end-effector position is at (2.0, 0, 0):
print("arm.angles: ", arm.angles)
print("arm.ee: ", arm.ee)
#Sets the joint angles to 30 and 60 degrees to calculate a new position of the end-effector:
arm.angles = [np.pi / 6, np.pi / 3] # or np.deg2rad([30, 60])
print("arm.ee: ", arm.ee)
#Sets a position of the end-effector to calculate the joint angles:
arm.ee = [2 / np.sqrt(2), 2 / np.sqrt(2), 0.]
print("arm.angles: ", arm.angles)
print("rad2 degrees: ", np.round(np.rad2deg(arm.angles)))
import tinyik
import numpy as np
np.set_printoptions(suppress=True)
arm = tinyik.Actuator([
'z', [0, 0, 60], 'x', [0, 0., 120.], 'x', [0., 0., 120.], 'x',
[0., 0., 180]
])
## 90-theta
def convertAnglesToBraccio(angles):
##base, shoulder,arm,wrist
[ang1, ang2, ang3, ang4] = angles
newAngs = [90 - ang for ang in angles]
return newAngs
def convertAnglesFromBraccio(angles):
##base, shoulder,arm,wrist
[ang1, ang2, ang3, ang4] = angles
newAngs = [90 - ang for ang in angles]
LS_z_u =
LS_z_d =
LS_puntita =
LS_GPIO = [LS_h_l, LS_h_d, LS_rc_l, LS_rc_d, LS_z_u, LS_z_d, LS_puntita] # incluir todos los LS
LS_stop = False
'''Algorithm for controlling SCARA robot.
from cartesian (x,y,z) to (x',y',z')
x axis is positive in the direction of maximum stretching
y axis is positive counterclockwise
z <= 0'''
## SCARA parameters
L1 = 330.15 # Humero [mm]
L2 = 338.0 # radio-cubito [mm]
arm = tinyik.Actuator(['z', [L1, 0., 0.], 'z', [L2, 0., 0.]])
ID_h = "" #INCLUIR
#odrv_h = odrive.find_any(ID_h)
cuentas_h_ang = 6000/360 # cuantas cuentas equivalen a un grado? [cuentas/grado]. VERIFICAR
ID_cz = "" #INCLUIR
#odrv_cz = odrive.find_any(ID_cz)
cuentas_c_ang = 6000/360 # cuantas cuentas equivalen a un grado? [cuentas/grado]. VERIFICAR
vueltas_z_mm = 2 # cuantas vueltas da por milimtro avanzado [vueltas/mm]. VERIFICAR
cuentas_z_vuelta = 6000 # cuentas cuentas realiza al girar 360 grados [cuentas/vuelta]. VERIFICAR
# Asumamos que parte en 10,0,0, producto de haber realizado rutina de homing
# ARREGAR ESTOS VALORES, TINYIK ASUME QUE EL ANGULO 0,0 EQUIVALE AL BRAZO ESTIRADO AL COMPLETO
x = 10
y = 0
z = 0
arm.ee = [x, y, z]
print('condicion inicial')
import tinyik as ti
import numpy as np
arm = ti.Actuator(['z', [1.0, 0.0, 0.0], 'z', [1.0, 0.0, 0.0]])
print(arm.angles, "\n")
print(arm.ee, "\n")
#forward kinematics
arm.angles = [np.pi / 6, np.pi / 3]
print(arm.ee, "\n")
#reserse kinematics
x = float(input())
y = float(input())
l = np.sqrt((x * x) + (y * y))
x = 0
if l <= 2: #for points within the hemisphere of radius 2
x = 1
if x == 1:
arm.ee = [x, y, 0.0]
#to account for points that require the joint angle to be less than 90 degree
if arm.angles[0] <= 1.5707933 and arm.angles[
0] >= -1.5707933: #pi/2=1.5707933
print(arm.angles, "\n")
print(np.round(np.rad2deg(arm.angles)), "\n")
else:
print("The given coordinates is not accessible")
else:
print("The given coordinates are out of bound")
