Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
executable file 99 lines (79 sloc) 2.53 KB
##################################################
# Inverse Kinematics
# This is an example to build up a robot arm
# using (modified?) DH parameters
# then use the InverseKinematics3D service to
# compute the forward and inverse kinematics for
# the arm.
##################################################
from time import sleep
from org.myrobotlab.service import InMoovArm
from org.myrobotlab.kinematics import DHRobotArm
from org.myrobotlab.kinematics import DHLink
from org.myrobotlab.kinematics import DHLinkType
# Create a robot arm
myRobotArm = DHRobotArm()
# Lets create a 2 link robot arm
# Create the first link in the arm specified by 100,100,0,90
# additionally specify a name for the link which can be used elsewhere.
d1 = 100
r1 = 100
theta = 0
alpha = 90
link0 = DHLink("base", d1, r1, theta, alpha)
# Create the second link (same as the first link.)
d1 = 100
r1 = 100
theta = 0
alpha = 90
link1 = DHLink("link1", d1, r1, theta, alpha)
# Add the links to the robot arm
myRobotArm.addLink(link0)
myRobotArm.addLink(link1)
# create the IK3D service.
ik3d= Runtime.createAndStart("ik3d", "InverseKinematics3D")
# assign our custom DH robot arm to the IK service.
ik3d.setCurrentArm(myRobotArm)
# print out the current postion of the arm.
print ik3d.getCurrentArm().getPalmPosition()
# Now, pick a start/end point
# and begin moving along a stright line as specified by the start/stop point.
# starting point
# x , y , z
x1 = 100
y1 = 100
z1 = 100
# ending point
# x , y , z
x2 = 200
y2 = -400
z2 = 100
# how many steps will we take to get there
numSteps = 100
# delay between steps (in seconds) (this will control the velocity of the end effector.
delay = 0.1
# lets compute how long the path is.
# this is the change in x,y,z between the two points
# divided up into numSteps, so we know how much to
# move in the x,y,z direction for each step.
dx = 1.0*(x2 - x1)/numSteps
dy = 1.0*(y2 - y1)/numSteps
dz = 1.0*(z2 - z1)/numSteps
# our starting point for the iteratin
# set that to the current x,y,z position
curX = x1
curY = y1
curZ = z1
# tell the arm to configure to that point
ik3d.moveTo(curX,curY,curZ)
# iterate over the 100 steps
for i in range(0 , 100):
# Increment our desired current position by dx,dy,dz
curX+=dx
curY+=dy
curZ+=dz
# tell the ik engine to move to that new point
ik3d.moveTo(curX, curY, curZ)
# pause for a moment to let the arm arrive at it's destination
# smaller delay = faster movement.
sleep(delay)
You can’t perform that action at this time.