def advance(self,dt):
"""Move the end effector in a circle, maintaining its orientation
dt is the time step.
"""
model = self.robot_model
controller = self.robot_controller
qcur = controller.configToKlampt(controller.commandedPosition())
# TODO: implement me
if not controller.isMoving():
#done with prior motion
# model.randomizeConfig()
# q = model.getConfig()
# duration = 2
# controller.setPiecewiseLinear([duration],[controller.configFromKlampt(q)])
#setPosition does an immediate position command
# self.robot_controller.setPosition(self.robot_controller.configFromKlampt(q))
pen_axis_ik = ik.fixed_rotation_objective(self.end_effector, local_axis=self.pen_local_axis)
z_axis_ik = ik.fixed_rotation_objective(self.end_effector, world_axis=[0,0,1])
next_pos = self.cur_pos(self.time + dt)
point_ik = ik.objective(self.end_effector, local=self.end_effector.getLocalPosition(self.cur_position), world=next_pos)
ik.solve([pen_axis_ik, z_axis_ik, point_ik])
q = model.getConfig()
controller.setPiecewiseLinear([dt], [controller.configFromKlampt(q)])
self.time += dt
self.cur_position = next_pos
q = self.normalize_config(q, qcur)
model.setConfig(q)
return
def advance(self,dt):
"""The state machine logic of advance() should:
1. move the pen to an approach point near the start point of path 0
(The approach point should be lifted `lift_amount` distance away from the plane)
2. lower the pen to the start point of path 0
3. trace path 0, keeping the pen's local axis paralle to the plane axis
4. raise the pen by lift_amount
5. move in a straight joint space path to an approach point near path 1's start point
6. ...likewise, repeat steps 1-4 for the remaining paths
7. move back to qhome along a straight joint space path.
dt is the time step.
"""
model = self.robot_model
controller = self.robot_controller
qcur = controller.configToKlampt(controller.commandedPosition())
#TODO: implement me
if not controller.isMoving():
self.wait += 1
if self.wait < 50:
return
self.wait = 0
#done with prior motion
# model.randomizeConfig()
model.setConfig(normalize_config(model.getConfig(),qcur)) #this is needed to handle the funky spin joints
pen_axis_ik = ik.fixed_rotation_objective(self.end_effector, local_axis=self.pen_local_axis)
if self.state == "moveinit":
if self.path_index >= len(self.paths):
return
point_ik = ik.objective(self.end_effector, local=self.end_effector.getLocalPosition(self.cur_position), world=self.paths[self.path_index].milestones[0])
ik.solve([pen_axis_ik, point_ik])
q = model.getConfig()
controller.setPiecewiseLinear([dt],[controller.configFromKlampt(q)])
self.state = "writepath"
self.path_progress += 1
self.cur_position = self.paths[self.path_index].milestones[0]
elif self.state == "writepath":
if self.path_progress < len(self.paths[self.path_index]):
point_ik = ik.objective(self.end_effector, local=self.end_effector.getLocalPosition(self.cur_position), world=self.paths[self.path_index].milestones[self.path_progress])
ik.solve([pen_axis_ik, point_ik])
q = model.getConfig()
controller.setPiecewiseLinear([dt],[controller.configFromKlampt(q)])
self.path_progress += 1
self.cur_position = self.paths[self.path_index].milestones[0]
else:
self.state = "endpath"
elif self.state == "endpath":
self.path_progress = 0
self.state == "moveinit"
self.path_index += 1
# duration = 2
# controller.setPiecewiseLinear([duration],[controller.configFromKlampt(q)])
#setPosition does an immediate position command
#self.robot_controller.setPosition(self.robot_controller.configFromKlampt(q))
return
def solve(self, target_loc):
if isinstance(target_loc, np.ndarray):
target_loc = list(target_loc.flat)
if target_loc[0] < 0:
initial_cfg = self.initial_cfg_left
else:
initial_cfg = self.initial_cfg_right
self.robot.setConfig(initial_cfg)
objective = ik.objective(self.grasptarget_link,
local=[0, 0, 0],
world=target_loc)
flag = ik.solve(objective, iters=1000, tol=1e-4)
cfg = self.robot.getConfig()[7:14]
return flag, cfg, initial_cfg[7:14]
def ik(self, from_config, to_ee_loc, return_flag=False):
if isinstance(to_ee_loc, np.ndarray):
to_ee_loc = list(to_ee_loc.flat)
from_config_full = copy(self.robot.getConfig())
from_config_full[7:14] = from_config
self.robot.setConfig(from_config_full)
objective = ik.objective(self.grasptarget_link,
local=[0, 0, 0],
world=to_ee_loc)
flag = ik.solve(objective, iters=1000, tol=1e-4)
cfg = self.robot.getConfig()[7:14]
if not return_flag:
return cfg
else:
return cfg, flag
def retract(robot,gripper,amount,local=True):
"""Retracts the robot's gripper by a vector `amount`.
if local=True, amount is given in local coordinates. Otherwise, its given in
world coordinates.
"""
if not isinstance(gripper,(int,str)):
gripper = gripper.base_link
link = robot.link(gripper)
Tcur = link.getTransform()
if local:
amount = so3.apply(Tcur[0],amount)
obj = ik.objective(link,R=Tcur[0],t=vectorops.add(Tcur[1],amount))
res = ik.solve(obj)
if not res:
return None
return robot.getConfig()
def solve(self, target_loc):
if isinstance(target_loc, np.ndarray):
target_loc = list(target_loc.flat)
if target_loc[0] < 0:
initial_cfg = self.initial_cfg_left
else:
initial_cfg = self.initial_cfg_right
self.robot.setConfig(initial_cfg)
# return True, self.robot.getConfig()[7:14]
objective = ik.objective(self.grasptarget_link,
local=[0, 0, 0],
world=target_loc)
flag = ik.solve(objective, iters=1000, tol=1e-4)
cfg = self.robot.getConfig()[7:14]
# collisions = list(self.collider.robotSelfCollisions(self.robot))
# print(f'get {len(collisions)} collisions')
# for c1, c2 in collisions:
# print('collisions', c1.getName(), c2.getName())
return flag, cfg, initial_cfg[7:14]
def configSolver(self, robot, catch_location):
'''
solve for all the possibilities and find the one with lowest cost compare with the current config
'''
# Pre-calculated waiting and prepare configs
if tuple(catch_location) in PRE_CAL_CONFIG.keys():
return [PRE_CAL_CONFIG[tuple(catch_location)], 0.0]
current_config = robot.getConfig()
opt_sol = [current_config, float('inf')]
# norm solver
for link_idx in range(4, robot.numLinks() - 1):
cur_end_link = robot.link(link_idx)
for link_pos in [0.02 * mul for mul in range(1)]:
cur_goal = ik.objective(cur_end_link,
local=[(link_pos, 0, 0)],
world=[catch_location])
if ik.solve(cur_goal):
cur_sol = robot.getConfig()
cur_dist = robot.distance(cur_sol, current_config)
if cur_dist < opt_sol[1]:
opt_sol[0], opt_sol[1] = cur_sol, cur_dist
return opt_sol
def ik_solve(target_position, first_time=False):
left_ee_link = robot.link(13)
left_active_dofs = [7, 8, 9, 10, 11, 12]
ee_local_pos = [0, 0, 0]
h = 0.1
local = [ee_local_pos, vectorops.madd(ee_local_pos, (1, 0, 0), -h)]
world = [target_position, vectorops.madd(target_position, (0, 0, 1), h)]
goal = ik.objective(left_ee_link, local=local, world=world)
if first_time:
solved = ik.solve_global(goal,
activeDofs=left_active_dofs,
startRandom=True)
else:
solved = ik.solve(goal, activeDofs=left_active_dofs)
if solved:
print("solve success")
return True
else:
print("Solve unsuccessful")
return False
def getTorque(t,q,dq):
""" TODO: implement me
Returns a 4-element torque vector given the current time t, the configuration q, and the joint velocities dq to drive
the robot so that it traces out the desired curves.
Recommended order of operations:
1. Monitor and update the current status and stroke
2. Update the stroke_progress, making sure to perform smooth interpolating trajectories
3. Determine a desired configuration given current state using IK
4. Send qdes, dqdes to PID controller
"""
global robot,status,current_stroke,current_stroke_progress,stroke_list, pid_integrators, interp, prevTarget, interpIncrement
qdes = [0,0,0,0]
dqdes = [0,0,0,0]
link = robot.link(3)
if (status == "pen up") :
print "PEN UP"
penHeight = 0.05
elif (status == "pen up moving") :
print "PEN UP MOVING"
penHeight = 0.05
elif (status == "pen down") :
print "PEN DOWN"
penHeight = -0.002
else :
print status
penHeight = -0.002
if (current_stroke == len(stroke_list)):
status == "done"
penHeight = 0.2
target = (0, 0)
else :
target = stroke_list[current_stroke][current_stroke_progress]
if (status == "move") :
print "want to interp: ", interp
r1 = prevTarget[0] + interp * (target[0] - prevTarget[0])
r2 = prevTarget[1] + interp * (target[1] - prevTarget[1])
target = (r1,r2)
print target
wc = []
wc2 = []
wc.extend(target)
wc.extend([penHeight])
wc2.extend(target)
wc2.extend([penHeight+1])
obj = ik.objective(link,local= [[0,0,0],[0,0,-1]], world=[wc, wc2])
res = ik.solve(obj)
qdes = robot.getConfig()
kP = [10,5,2,50]
kI = [0,0,0,0]
kD = [8,1.5,0.1,5]
allWithin = True
for i in range(len(q)):
acceptableRange = 0.01
if (status == "moving" and interp <0.8) :
acceptableRange = 0.05
if (status == "moving" and interp >0.8) :
acceptableRange = 0.001
if (math.fabs(q[i] - qdes[i]) > acceptableRange):
allWithin = False
if allWithin:
print "ALL WITHIN"
if (status == "done") :
print "do nothing"
elif (status == "pen up") :
interp = 0
status = "pen up moving"
current_stroke_progress = 0;
current_stroke = current_stroke + 1
elif (status == "pen up moving") :
status = "pen down"
elif (status == "pen down") :
interp = interpIncrement
status = "move"
prevTarget = target
print "prevTarget: ", prevTarget
current_stroke_progress = current_stroke_progress + 1
elif (status == "move"):
if interp < 1 - interpIncrement + interpIncrement/2:
interp = interp + interpIncrement
else:
pid_integrators = [0,0,0,0]
if (current_stroke_progress == len(stroke_list[current_stroke]) -1):
print "END OF LIST"
print current_stroke_progress
status = "pen up"
else :
print "KEEP GOING"
interp = interpIncrement
prevTarget = target
print "prevTarget: ", prevTarget
print current_stroke_progress
current_stroke_progress = current_stroke_progress+1
return getPIDTorqueAndAdvance(q,dq,qdes,dqdes,kP,kI,kD,pid_integrators,dt)
def getTorque(t,q,dq):
""" TODO: implement me
Returns a 4-element torque vector given the current time t, the configuration q, and the joint velocities dq to drive
the robot so that it traces out the desired curves.
Recommended order of operations:
1. Monitor and update the current status and stroke
2. Update the stroke_progress, making sure to perform smooth interpolating trajectories
3. Determine a desired configuration given current state using IK
4. Send qdes, dqdes to PID controller
"""
global robot,status,current_stroke,current_stroke_progress,stroke_list, pid_integrators, interp, prevTarget, interpIncrement, t_up
qdes = [0,0,0,0]
dqdes = [0,0,0,0]
link = robot.link(3)
penDrawHeight = -0.001
if (status == "pen up") :
penHeight = 0.005
elif (status == "pen up moving") :
penHeight = 0.005
elif (status == "pen down") :
#penHeight = -0.002
penHeight = penDrawHeight
elif (status == "done") :
penHeight = 0.005
else :
#penHeight = -0.002
penHeight = penDrawHeight
if (current_stroke == len(stroke_list)):
status == "done"
penHeight = 0.2
target = prevTarget
else :
target = stroke_list[current_stroke][current_stroke_progress]
if (status == "move") :
if interp > 0.9:
delta = 0.08
interp = 1 + delta
r1 = prevTarget[0] + interp * (target[0] - prevTarget[0])
r2 = prevTarget[1] + interp * (target[1] - prevTarget[1])
target = (r1,r2)
wc = []
wc2 = []
wc.extend(target)
wc.extend([penHeight])
wc2.extend(target)
wc2.extend([penHeight+1])
# IK Solver
obj = ik.objective(link,local= [[0,0,0],[0,0,-1]], world=[wc, wc2])
res = ik.solve(obj)
qdes = robot.getConfig()
kP = [20,8,1,20]
kI = [0.05,0.01,0,0]
kD = [8,1.5,0.1,5]
allWithin = True
for i in range(len(q)):
acceptableRange = 0.002
if (status == "move" and interp <0.75) :
acceptableRange = 0.01
if (status == "move" and interp >0.75) :
acceptableRange = 0.001
if (math.fabs(q[i] - qdes[i]) > acceptableRange):
allWithin = False
if allWithin:
if (status == "done") :
status = "done"
elif (status == "pen up") :
interp = 0
status = "pen up moving"
current_stroke_progress = 0;
current_stroke = current_stroke + 1
elif (status == "pen up moving") :
status = "pen down"
elif (status == "pen down") :
interp = interpIncrement
status = "move"
prevTarget = target
current_stroke_progress = current_stroke_progress + 1
elif (status == "move"):
if interp < (1 -.05):
interp = interp + interpIncrement
else:
pid_integrators = [0,0,0,0]
if (current_stroke_progress == len(stroke_list[current_stroke]) -1):
t_up = t
status = "pen up"
if (current_stroke == len(stroke_list)-1) :
status = "done"
else :
interp = interpIncrement
prevTarget = target
current_stroke_progress = current_stroke_progress+1
return getPIDTorqueAndAdvance(q,dq,qdes,dqdes,kP,kI,kD,pid_integrators,dt)
