def partitionFn(d, value, x):
#HACK: for pendulum
if SO2_HACK and d == 0:
math.copysign(1, so2.diff(x[d], value))
#HACK: for SE2
if SE2_HACK and d == 2:
math.copysign(1, so2.diff(x[d], value))
return math.copysign(1, x[d] - value)
def normalize_config(config,reference):
"""For a configuration that may have wildly incorrect continuous-rotation
joints, this calculates a version of config that represents the same
configuration but is within +/ pi radians of the reference configuration.
"""
res = [v for v in config]
for i in spin_joints:
if abs(config[i]-reference[i]) > math.pi:
d = so2.diff(config[i]%(math.pi*2),reference[i])
res[i] = reference[i] + d
return res
def pdcontrol(t, q, dq):
"""Standard PD controller"""
return PD([so2.diff(q[0], qdes[0]), q[1] - qdes[1]], dq, KP, KD)
def pdcontrol(t, q, dq):
"""Standard PD controller"""
return PD(so2.diff(q, qdes), dq, KP, KD)
def interpolate(self, a, b, u):
return vectorops.interpolate(a[:2], b[:2],
u) + [a[2] + u * so2.diff(b[2], a[2])]
def distance(self, a, b):
# need to set the angle weight
return vectorops.distance(a[:2],
b[:2]) + abs(so2.diff(a[2], b[2])) * 0.1
def pdcontrol(t,q,dq):
"""Standard PD controller for cartpole problem"""
return PD([q[0]-qdes[0],so2.diff(q[1],qdes[1])],dq,KP,KD)