def __init__(self):
# TODO: complete the DH parameters
self._lengths = [.80, # Shoulder to elbow length
.80, # Elbow to wrist length
.68] # Wrist to hand length
self._joints = {frames.SYMB("b1"): 0.0,
frames.SYMB("s1"): 0.0,
frames.SYMB("e1"): 0.0,
frames.SYMB("g1"): 0.0}
# Connect the servos to their pin and select the range of the angles
self.servos = {"base": ServoSB(11, -90, 90),
"shoulder": ServoSB(13, -90, 90),
"elbow": ServoSB(7, -90, 90),
"grip": ServoSB(15, 0, 90)}
# Create the body using multilink code and DH parameters
self._body = MultiLink()
_dh = {"d": [.0],
"theta": ["s:b1"],
"r": [self._lengths[0]],
"alpha": [.0]
}
self._body.fromDH(_dh)
self._body.compose(*self._joints.keys())
self._J = self._body.J.copy()
def compose(self, *kargs):
self.addLink(0, .0) # Add effector reference frame
self.H = self.tt.compose()
self.H_eff = self.H * self.eff
symbols = [frames.SYMB(s) for s in kargs]
if symbols:
self.J = self.H_eff.jacobian(symbols)
def compose(self, *kargs):
self.H = self.tt.compose()
self.H_eff = self.H * self.eff
symbols = [frames.SYMB(s) for s in kargs]
if symbols:
self.J = self.H_eff.jacobian(symbols)
arm3.compose()
arm3.plotLinks()
plt.savefig("links2.png")
arm2 = MultiLink()
_dh1 = (.0, "s:alpha", "s:L1", .0)
_dh2 = (.0, "s:beta", "s:L2", .0)
a2l1 = arm2.addLink(_dh1)
a2l2 = arm2.addLink(_dh2)
print arm2.tt
arm2.compose("alpha", "beta")
arm2.bindSymbols({
frames.SYMB("alpha"): 2.0,
frames.SYMB("beta"): 22.0,
frames.SYMB("L1"): 1.0,
frames.SYMB("L2"): 1.0
})
arm2.plotLinks(verbose=False)
print arm2.jointsToPosition(alpha=45.0, beta=-10, L1=1.0, L2=1.0)
#print arm2.tt
#print arm2.H_eff
Js = arm2.getJacobian()
display(Js)
display(arm2.J)
#print arm2.J
def _s(s):
return frames.SYMB(s)
def jointsToPosition(self, *kargs, **kwargs):
""" This is the direct kinematic """
d = {}
for k, v in kwargs.items():
d[frames.SYMB(k)] = v
return self.H_eff.subs(d).evalf()
a2 = -10.0
l3 = 1.5
a3 = -30.0
arm = MultiLink()
link1 = arm.addLink(l1, a1)
link2 = arm.addLink(l2, a2)
link3 = arm.addLink(l3, a3)
arm.compose()
arm.plotLinks()
plt.savefig("links.png")
arm2 = MultiLink()
a2l1 = arm2.addLink("s:L1", "s:alpha")
a2l2 = arm2.addLink("s:L2", "s:beta")
arm2.compose("alpha", "beta")
print arm2.jointsToPosition(alpha=45.0, beta=-10, L1=1.0, L2=1.0)
#print arm2.tt
#print arm2.H_eff
Js = arm2.J.copy().subs({
frames.SYMB("alpha"): 45.0,
frames.SYMB("beta"): -10,
frames.SYMB("L1"): 1.0,
frames.SYMB("L2"): 1.0
})
#print arm2.J
print Js.evalf()
print arm3.tt
arm3.compose()
arm3.plotLinks()
plt.savefig("links2.png")
arm2 = MultiLink()
_dh1 = (.0,"s:alpha","s:L1",.0)
_dh2 = (.0,"s:beta","s:L2",.0)
a2l1 = arm2.addLink(_dh1)
a2l2 = arm2.addLink(_dh2)
print arm2.tt
arm2.compose("alpha","beta")
arm2.bindSymbols({frames.SYMB("alpha"):2.0,frames.SYMB("beta"):22.0,
frames.SYMB("L1"):1.0,frames.SYMB("L2"):1.0
})
arm2.plotLinks(verbose=False)
print arm2.jointsToPosition(alpha=45.0,
beta=-10,
L1=1.0,
L2=1.0)
#print arm2.tt
#print arm2.H_eff
Js = arm2.getJacobian()
display(Js)
display(arm2.J)