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 set_q(self, q, debug=False):
torso_x, torso_y, yaw_rads = self.estimate_torso_xy_yaw_rads_from_stance(
q)
if not torso_x:
Logger.log(("stance: end config"), "OKGREEN")
return
torso_z = parameters.TORSO_Z_DESIRED
fl_global = self.adjust_endeff_z(self.scatter_list[q[0]][0:3])
fr_global = self.adjust_endeff_z(self.scatter_list[q[1]][0:3])
br_global = self.adjust_endeff_z(self.scatter_list[q[2]][0:3])
bl_global = self.adjust_endeff_z(self.scatter_list[q[3]][0:3])
if debug:
print(" fl_global:", Logger.pp_list(fl_global))
print(" fr_global:", Logger.pp_list(fr_global))
print(" br_global:", Logger.pp_list(br_global))
print(" bl_global:", Logger.pp_list(bl_global))
f_l_r_const = ik.objective(self.fl_end_effector,
R=self.get_end_effector_rotation_matrix(
1, yaw_rad=yaw_rads),
t=fl_global)
f_r_r_const = ik.objective(self.fr_end_effector,
R=self.get_end_effector_rotation_matrix(
2, yaw_rad=yaw_rads),
t=fr_global)
b_l_r_const = ik.objective(self.bl_end_effector,
R=self.get_end_effector_rotation_matrix(
3, yaw_rad=yaw_rads),
t=bl_global)
b_r_r_const = ik.objective(self.br_end_effector,
R=self.get_end_effector_rotation_matrix(
4, yaw_rad=yaw_rads),
t=br_global)
global_torso_xyz = [torso_x, torso_y, torso_z]
des_torso_rotation = self.get_torso_R_from_yaw_rad(yaw_rads)
torso_obj = ik.objective(self.torso,
R=des_torso_rotation,
t=global_torso_xyz)
bias_config = utils.nominal_config()
goals = [f_l_r_const, f_r_r_const, b_l_r_const, b_r_r_const, torso_obj]
s = IKSolver(self.robosimian)
for goal in goals:
s.add(goal)
s.setBiasConfig(bias_config)
res = s.solve()
if not res:
print("robot_poser IK Error")
return False
return True
def compute_initial_final(world, robot):
"""
Initial point is fixed and given, target is set by
1. rotate q0 a random angle between -pi/4, 3*pi/4
2. random perturb ee position by 0.3 m
If IK fails, simply abandon it.
Exit when 10 plans are generated.
"""
end_effector = robot.link(7)
space = makeSpace(world, robot)
# we want to the end effector to move from start to goal
start = [-0.2, -0.50, 1.4]
# solve IK for the start point
unit_vec1 = [0.0, -0.1, 0.0]
null = [0.0, 0.0, 0.0]
unit_vec2 = [0.0, 0., -0.1]
objective = ik.objective(end_effector,
local=[[0, 0, 0], unit_vec1],
world=[start, vo.madd(start, unit_vec2, 1)])
collision_check = lambda: (space.selfCollision(robot.getConfig(
))) == False and (space.envCollision(robot.getConfig()) == False)
ikflag = ik.solve_global(objective,
feasibilityCheck=collision_check,
startRandom=True,
numRestarts=500)
if ikflag == False:
raise Exception("IK for start point failed!")
qstart = robot.getConfig()
print('Feasible ik start ', robot.getConfig())
# then IK for goal
qtmp = qstart[:]
qtmp[1] += np.random.random(
) * np.pi + np.pi / 2 # rotation angle, pi/2 to 3pi/2
robot.setConfig(qtmp)
eepos = np.array(end_effector.getWorldPosition(null))
rand_pos = eepos.copy()
rand_pos[:2] += np.random.random(2) * 0.3 - 0.15
rand_pos[2] += (np.random.random() * 0.8 - 0.4
) # this might be too large, we will see
goal = rand_pos.tolist()
objective = ik.objective(end_effector,
local=[[0, 0, 0], unit_vec1],
world=[goal, vo.madd(goal, unit_vec2, 1)])
ikflag = ik.solve_global(objective,
feasibilityCheck=collision_check,
startRandom=True,
numRestarts=500)
if ikflag == False:
raise Exception("IK for goal point failed!")
print("Feasible ik goal ", robot.getConfig())
qgoal = robot.getConfig()
return np.array(qstart), np.array(qgoal)
def set_q(self, q, debug=False):
torso_x, torso_y, yaw_rads = self.get_torso_xy_yaw(q)
if not torso_x:
return
torso_z = parameters.TORSO_Z_DESIRED
fl_global = self.replace_end_effector_z(self.scatter_list[q[0]][0:3])
fr_global = self.replace_end_effector_z(self.scatter_list[q[1]][0:3])
br_global = self.replace_end_effector_z(self.scatter_list[q[2]][0:3])
bl_global = self.replace_end_effector_z(self.scatter_list[q[3]][0:3])
if debug:
print(" fl_global:", Logger.pp_list(fl_global))
print(" fr_global:", Logger.pp_list(fr_global))
print(" br_global:", Logger.pp_list(br_global))
print(" bl_global:", Logger.pp_list(bl_global))
f_l_r_const = ik.objective(self.f_l,
R=self.get_desired_end_effector_rotation(
1, yaw_rads),
t=fl_global)
f_r_r_const = ik.objective(self.f_r,
R=self.get_desired_end_effector_rotation(
2, yaw_rads),
t=fr_global)
b_l_r_const = ik.objective(self.b_l,
R=self.get_desired_end_effector_rotation(
3, yaw_rads),
t=bl_global)
b_r_r_const = ik.objective(self.b_r,
R=self.get_desired_end_effector_rotation(
4, yaw_rads),
t=br_global)
global_torso_xyz = [torso_x, torso_y, torso_z]
torso_obj = ik.objective(self.torso,
local=[0, 0, 0],
world=global_torso_xyz)
bias_config = utils.nominal_config()
goals = [f_l_r_const, f_r_r_const, b_l_r_const, b_r_r_const, torso_obj]
s = IKSolver(self.robosimian)
for goal in goals:
s.add(goal)
s.setBiasConfig(bias_config)
res = s.solve()
if not res:
return False
return True
def solve_ik(robotlink, localpos, worldpos, index):
"""
:param robotlink: {Klampt.RobotModelLink} -- the Klampt robot link model
:param localpos: {list} -- the list of points in the robot link frame
:param worldpos: {list} -- the list of points in the world frame
:param index: {int} -- the index number
:return: robot.getConfig(): {list} -- the list of the robot configuration
"""
robot = robotlink.robot()
space = robotcspace.RobotCSpace(robot)
obj = ik.objective(robotlink, local=localpos, world=worldpos)
maxIters = 100
tol = 1e-8
for i in range(1000):
s = ik.solver(obj, maxIters, tol)
res = s.solve()
if res and not space.selfCollision() and not space.envCollision():
return robot.getConfig()
else:
print(
"Couldn't solve IK problem in " + str(index) +
"th. Or the robot exists self-collision or environment collision."
)
s.sampleInitial()
def mousefunc(self,button,state,x,y):
#Put your mouse handler here
#the current example prints out the list of objects clicked whenever
#you right click
GLWidgetProgram.mousefunc(self,button,state,x,y)
self.reSolve = False
dragging = False
if NEW_KLAMPT:
dragging = self.widgetPlugin.klamptwidgetdragging
else:
dragging = self.draggingWidget
if not dragging and button == 2 and state==0:
#down
clicked = self.click_world(x,y)
if clicked is not None and isinstance(clicked[0],RobotModelLink):
#make a new widget
link, wpt = clicked
lpt = se3.apply(se3.inv(link.getTransform()),wpt)
self.ikIndices.append(len(self.ikWidgets))
self.ikWidgets.append(PointPoser())
self.ikWidgets[-1].set(wpt)
self.widgetMaster.add(self.ikWidgets[-1])
self.ikProblem.addObjective(ik.objective(link,local=lpt,world=wpt))
GLWidgetProgram.mousefunc(self,button,state,x,y)
self.refresh()
return
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 mousefunc(self, button, state, x, y):
#Put your mouse handler here
#the current example prints out the list of objects clicked whenever
#you right click
GLWidgetProgram.mousefunc(self, button, state, x, y)
self.reSolve = False
dragging = False
if NEW_KLAMPT:
dragging = self.widgetPlugin.klamptwidgetdragging
else:
dragging = self.draggingWidget
if not dragging and button == 2 and state == 0:
#down
clicked = self.click_world(x, y)
if clicked is not None and isinstance(clicked[0], RobotModelLink):
#make a new widget
link, wpt = clicked
lpt = se3.apply(se3.inv(link.getTransform()), wpt)
self.ikIndices.append(len(self.ikWidgets))
self.ikWidgets.append(PointPoser())
self.ikWidgets[-1].set(wpt)
self.widgetMaster.add(self.ikWidgets[-1])
self.ikProblem.addObjective(
ik.objective(link, local=lpt, world=wpt))
GLWidgetProgram.mousefunc(self, button, state, x, y)
self.refresh()
return
def set_pose(self, r_pose):
torso_xyz_yawdeg = r_pose.torso_xyz_yawdeg
torso_x, torso_y, torso_z, torso_yaw_deg = (
torso_xyz_yawdeg[0],
torso_xyz_yawdeg[1],
torso_xyz_yawdeg[2],
torso_xyz_yawdeg[3],
)
fl_xyz, fr_xyz, br_xyz, bl_xyz = r_pose.fl, r_pose.fr, r_pose.br, r_pose.bl
yaw_rads = np.deg2rad(torso_yaw_deg)
f_l_rotation = self.get_end_effector_rotation_matrix(1)
f_r_rotation = self.get_end_effector_rotation_matrix(2)
b_r_rotation = self.get_end_effector_rotation_matrix(3)
b_l_rotation = self.get_end_effector_rotation_matrix(4)
f_l_r_const = ik.objective(self.fl_end_effector,
R=f_l_rotation,
t=fl_xyz[0:3])
f_r_r_const = ik.objective(self.fr_end_effector,
R=f_r_rotation,
t=fr_xyz[0:3])
b_l_r_const = ik.objective(self.bl_end_effector,
R=b_r_rotation,
t=bl_xyz[0:3])
b_r_r_const = ik.objective(self.br_end_effector,
R=b_l_rotation,
t=br_xyz[0:3])
global_torso_xyz = [torso_x, torso_y, torso_z]
des_torso_rotation = self.get_torso_R_from_yaw_rad(yaw_rads)
torso_obj = ik.objective(self.torso,
R=des_torso_rotation,
t=global_torso_xyz)
goals = [f_l_r_const, f_r_r_const, b_l_r_const, b_r_r_const, torso_obj]
s = IKSolver(self.robosimian)
for goal in goals:
s.add(goal)
# s.setTolerance(ik_max_deviation)
s.setBiasConfig(utils.nominal_config())
res = s.solve()
if not res:
print("set_pose IK error")
return False
return True
def set_pose_w_R(self, r_pose):
torso_xyz_yawdeg = r_pose.torso_xyz_yawdeg
fl_xyz, fr_xyz, br_xyz, bl_xyz = r_pose.fl, r_pose.fr, r_pose.br, r_pose.bl
fl_obj = ik.objective(self.fl_end_effector, R=fl_xyz[3], t=fl_xyz[0:3])
fr_obj = ik.objective(self.fr_end_effector, R=fr_xyz[3], t=fr_xyz[0:3])
bl_obj = ik.objective(self.bl_end_effector, R=bl_xyz[3], t=bl_xyz[0:3])
br_obj = ik.objective(self.br_end_effector, R=br_xyz[3], t=br_xyz[0:3])
des_torso_rotation = self.get_torso_R_from_yaw_rad(
np.deg2rad(torso_xyz_yawdeg[3]))
torso_obj = ik.objective(self.torso,
R=des_torso_rotation,
t=torso_xyz_yawdeg[0:3])
bias_config = utils.nominal_config()
bias_config[0] = torso_xyz_yawdeg[0]
bias_config[1] = torso_xyz_yawdeg[1]
bias_config[2] = torso_xyz_yawdeg[2]
bias_config[3] = np.deg2rad(torso_xyz_yawdeg[3])
self.robosimian.setConfig(bias_config)
return ik.solve_global([torso_obj, fl_obj, fr_obj, bl_obj, br_obj])
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example prints the config when [space] is pressed
if c == 'h' or c == '?':
print("Controls:")
print("- [space]: print the widget's sub-robot configuration")
print("- r: randomize the sub-robot configuration")
print("- p: plan to the widget's sub-robot configuration")
print("- i: test the IK functions")
elif c == ' ':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
print("Config:", subconfig)
self.subrobots[0].setConfig(subconfig)
elif c == 'r':
self.subrobots[0].randomizeConfig()
self.robotWidget.set(self.robot.getConfig())
elif c == 'p':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
self.subrobots[0].setConfig(self.subrobots[0].fromfull(
self.startConfig))
settings = {
'type': "sbl",
'perturbationRadius': 0.5,
'bidirectional': 1,
'shortcut': 1,
'restart': 1,
'restartTermCond': "{foundSolution:1,maxIters:1000}"
}
plan = robotplanning.planToConfig(self.world,
self.subrobots[0],
subconfig,
movingSubset='all',
**settings)
plan.planMore(1000)
print(plan.getPath())
elif c == 'i':
link = self.subrobots[0].link(self.subrobots[0].numLinks() - 1)
print("IK solve for ee to go 10cm upward...")
obj = ik.objective(link,
local=[0, 0, 0],
world=vectorops.add(
link.getWorldPosition([0, 0, 0]),
[0, 0, 0.1]))
solver = ik.solver(obj)
res = solver.solve()
print(" result", res)
print(" residual", solver.getResidual())
print(self.robotWidget.set(self.robot.getConfig()))
else:
GLWidgetPlugin.keyboardfunc(self, c, x, y)
def set_end_effectors_xyz_torso_xyz_yaw(self, fl, fr, br, bl, torso_x,
torso_y, torso_z, yaw_rads):
global_torso_xyz = [torso_x, torso_y, torso_z]
des_torso_rotation = self.get_torso_R_from_yaw(yaw_rads)
torso_obj = ik.objective(self.torso,
R=des_torso_rotation,
t=global_torso_xyz)
f_l_r_const = ik.objective(self.f_l,
R=self.get_desired_end_effector_rotation(
1, yaw_rads),
t=fl)
f_r_r_const = ik.objective(self.f_r,
R=self.get_desired_end_effector_rotation(
2, yaw_rads),
t=fr)
b_l_r_const = ik.objective(self.b_l,
R=self.get_desired_end_effector_rotation(
3, yaw_rads),
t=bl)
b_r_r_const = ik.objective(self.b_r,
R=self.get_desired_end_effector_rotation(
4, yaw_rads),
t=br)
bias_config = utils.nominal_config()
goals = [f_l_r_const, f_r_r_const, b_l_r_const, b_r_r_const, torso_obj]
s = IKSolver(self.robosimian)
for goal in goals:
s.add(goal)
s.setBiasConfig(bias_config)
res = s.solve()
if not res:
print("robot_poser IK Error")
return False
return True
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 robot_move(mode,world,robot,link,point_ee,point_world,maxDev,IKErrorTolerence,
EEZLimit,collider,robotControlApi=None,ServoTime=9999.0,dt=1.0,
use_const = True,vis=vis,use_collision_detect = False,use_ik_detect = False):
robotCurrentConfig=klampt_2_controller(robot.getConfig())
goal=ik.objective(link,local=point_ee,world=point_world)
res=ik.solve_nearby(goal,maxDeviation=maxDev,tol=0.00001)
#res=ik.solve_global(goal,tol=0.00001)
if res:
# collision detect
if check_collision_linear(robot,collider,controller_2_klampt(robot,robotCurrentConfig),robot.getConfig(),10):
print "[!]Warning: collision detected!"
if use_collision_detect == True:
vis.show()
if input('continue?') != 1:
exit()
else:
pass
# cal difference
diff=np.max(np.absolute((np.array(vectorops.sub(robotCurrentConfig[0:5],klampt_2_controller(robot.getConfig())[0:5])))))
EEZPos=link.getTransform()[1]
if diff<IKErrorTolerence and EEZPos>EEZLimit: #126 degrees
if mode == 'debugging':
pass
elif mode == 'physical':
if use_const:
constantVServo(robotControlApi,ServoTime,klampt_2_controller(robot.getConfig()),dt)
else:
robotControlApi.setConfig(klampt_2_controller(robot.getConfig()))
else:
print "[!]IK too far away"
if use_ik_detect == True:
if input('continue?') != 1:
exit()
else:
diff = 9999.0
print "[!]IK failture"
if use_ik_detect == True:
vis.show()
if input('continue?') != 1:
exit()
return robot, diff
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(cur_target):
global robot
end_link = robot.link(robot.numLinks() - 1)
ee_local_axis = end_link.getAxis()
ee_local_position = (0, 0, 0)
cur_target_axis = (0, 0, -1)
goal = ik.objective(
end_link,
local=[
ee_local_position,
vectorops.add(ee_local_position, ee_local_axis)
], # match end points
world=[cur_target,
vectorops.add(cur_target, cur_target_axis)])
ik.solve_global(goal)
# ### Test Print
# print('Local Coord: ', [ee_local_position, vectorops.add(ee_local_position, (0,0,-0.01))])
# print('Global Coord: ', [cur_target, vectorops.add(cur_target, (0,0,0.01))])
# ###
return robot.getConfig()
def calculate_workspace_axis(robot,obstacles,end_effector,point_local,axis_local,axis_world):
global lower_corner,upper_corner
resolution = (15,15,15)
cellsize = vectorops.div(vectorops.sub(upper_corner,lower_corner),resolution)
invcellsize = vectorops.div(resolution,vectorops.sub(upper_corner,lower_corner))
reachable = np.zeros(resolution)
#TODO: your code here
for i in range(resolution[0]):
for j in range(resolution[1]):
for k in range(resolution[2]):
orig_config = robot.getConfig()
point = vectorops.add(vectorops.mul([i,j,k], cellsize), lower_corner)
world_constraint = ik.fixed_rotation_objective(end_effector, world_axis=axis_world)
local_constraint = ik.fixed_rotation_objective(end_effector, local_axis=axis_local)
point_goal = ik.objective(end_effector, local=point_local, world=point)
if ik.solve_global([point_goal, local_constraint, world_constraint], iters=10):
reachable[i,j,k] = 1.0
robot.setConfig(orig_config)
return reachable
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_solver(self, robot, x, q):
# check over all the links in the
for i in range(5):
# Objective: end effector on center of target, end of final link along target axis
objs = ik.objective(robot.link(6 - i),
local=[(0, 0, 0)],
world=[x])
s = model.ik.solver(objs)
# checks for solution with 100 potential tries (Restarts)
numRestarts = 100
solved = False
for i in range(numRestarts):
robot.setConfig(q)
## GLOBAL IMPROVEMENTS
#robot.setConfig(qglobal);
res = s.solve()
##now solve
if (res):
solved = True
# print "IK success!"
## EFFECIENCY TRACKING
#suc = suc + 1;
#print "success:", suc, "total:", tot;
## now the robot model's configuration is changed, and you need to
## extract it. Your errpos and erraxis values should be very small
## Get world axis for end effector
qseed = robot.getConfig()
## GLOBAL IMPROVEMENTS
#qglobal = qseed;
return (qseed, solved)
# if never solved, print failure
# print "IK failure... returning best solution found"
return (q, solved)
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
grasp_plate = grasp_plate.get_transformed(plate.getTransform()).transfer(
source_gripper, target_gripper)
end_effector = robot.link('EndEffector_Link')
h = 0.1
ee_local_pos = (0, 0, 0)
# we want the end effector to grasp from top of the object
ee_local_axis = source_gripper.primary_axis # (1, 0, 0)
point_local_array = np.array(ee_local_pos)
axis_local_array = np.array(ee_local_axis)
axis_world_array = np.array([0, 0, -1])
# grasp for swab
goal_pos_world = (0.5, -0.175, 0.71 + 0.17)
grasp_swab.ik_constraint = ik.objective(
end_effector,
local=[ee_local_pos,
list(point_local_array + h * axis_local_array)],
world=[goal_pos_world,
list(goal_pos_world + h * axis_world_array)])
# grasp for plate
goal_pos_world_plate = (0.68, 0.35, 0.87)
vis.add('', goal_pos_world_plate, color=[1, 0, 0, 1])
end_effector2 = robot2.link('EndEffector_Link')
grasp_plate.ik_constraint = ik.objective(
end_effector2,
local=[ee_local_pos,
list(point_local_array + h * axis_local_array)],
world=[
goal_pos_world_plate,
list(goal_pos_world_plate + h * axis_world_array)
])
def solve_qplace(self, Tplacement):
if self.robot0:
v = list(np.array(Tplacement[1]) - np.array([0.16, 0, 0]))
objective = ik.objective(self.robot.link(9), R=Tplacement[0], t=v)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved:
print('qplace')
return None
qpreplace = self.robot.getConfig()
# add a waypoint to insert swab into human face
face_trans = [0.34, -0.3, 1.05] # [0.5, -0.35, 1.35]
objective = ik.objective(self.robot.link(9),
R=so3.from_rpy((np.pi, -np.pi / 2, 0)),
t=face_trans)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved:
print('cannot place swab near mouth')
return None
self.qmouth = self.robot.getConfig()
self.qmouth = self.gripper.set_finger_config(
self.qmouth,
self.gripper.partway_open_config(self.close_amount))
face_trans = [0.34, -0.4, 1.05] # [0.5, -0.35, 1.35]
objective = ik.objective(self.robot.link(9),
R=so3.from_rpy((np.pi, -np.pi / 2, 0)),
t=face_trans)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7])
if not solved:
print('cannot insert swab into mouth')
return None
self.qmouth_in = self.robot.getConfig()
self.qmouth_in = self.gripper.set_finger_config(
self.qmouth_in,
self.gripper.partway_open_config(self.close_amount))
# add a waypoint to lower down the gripper
v1 = list(np.array(v) - np.array([0, 0, 0.1]))
# vis.add('v1', v1, color=[0, 0, 1, 1])
objective = ik.objective(self.robot.link(9),
R=so3.from_rpy(
(-np.pi / 2, 0, -np.pi / 2)),
t=v1)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved:
print('qlower')
return None
self.qlower = self.robot.getConfig()
self.qlower = self.gripper.set_finger_config(
self.qlower,
self.gripper.partway_open_config(self.close_amount))
# add waypoints to dip the swab into the plate
goal = (0.7, 0.35, 0.9)
t = list(np.array(goal) - np.array([0.16, 0, 0]))
objective = ik.objective(self.robot.link(9),
R=so3.from_rpy(
(-np.pi / 2, np.pi / 2, -np.pi / 2)),
t=t)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved:
print('cannot place swab into plate')
return None
self.qplaceplate = self.robot.getConfig()
self.qplaceplate = self.gripper.set_finger_config(
self.qplaceplate,
self.gripper.partway_open_config(self.close_amount))
t1 = list(np.array(t) - np.array([0, 0, 0.06]))
objective = ik.objective(self.robot.link(9),
R=so3.from_rpy(
(-np.pi / 2, np.pi / 2, -np.pi / 2)),
t=t1)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved:
print('cannot place swab into plate')
return None
self.qplaceplate_lower = self.robot.getConfig()
self.qplaceplate_lower = self.gripper.set_finger_config(
self.qplaceplate_lower,
self.gripper.partway_open_config(self.close_amount))
# add waypoints to throw the swab into the trash can
goal = (0.1, -0.4, 0.8)
t = list(np.array(goal) - np.array([0.16, 0, 0]))
objective = ik.objective(self.robot.link(9),
R=so3.from_rpy(
(-np.pi / 2, np.pi / 2, -np.pi / 2)),
t=t)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved:
print('cannot place swab into plate')
return None
self.qplace_trash = self.robot.getConfig()
self.qplace_trash = self.gripper.set_finger_config(
self.qplace_trash,
self.gripper.partway_open_config(self.close_amount))
self.qplace = self.gripper.set_finger_config(
self.qplace_trash, self.gripper.partway_open_config(1))
else:
Tplacement_x = se3.mul(Tplacement, se3.inv(self.Tobject_gripper))
objective = ik.objective(self.robot.link(9),
R=Tplacement_x[0],
t=Tplacement_x[1])
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved: return None
qpreplace = self.robot.getConfig()
qplace = self.gripper.set_finger_config(
qpreplace, self.gripper.partway_open_config(1))
return qplace
def keyboardfunc(self, c, x, y):
if c == 'h':
print('HELP:')
print('[right-click]: add a new IK constraint')
print('[space]: tests the current configuration')
print('d: deletes IK constraint')
print('t: adds a new rotation-fixed IK constraint')
print('f: flushes the current database to disk')
print('s: saves the current database to disk')
print('b: performs one background step')
print('B: starts / stops the background thread')
print('v: toggles display of the database')
print(
'c: toggles continuous re-solving of IK constraint its as being moved'
)
print('o: toggles soft / hard IK constraints')
elif c == ' ':
self.planningWorld.robot(0).setConfig(self.currentConfig)
soln = self.ikdb.solve(self.ikProblem)
if soln:
print("Solved")
self.currentConfig = soln
self.refresh()
else:
print("Failure")
elif c == 'd':
for i, w in enumerate(self.ikWidgets):
if w.hasHighlight():
print("Deleting IK widget")
#delete it
index = self.ikIndices[i]
self.widgetMaster.remove(w)
del self.ikWidgets[i]
del self.ikIndices[i]
del self.ikProblem.objectives[index]
for j in range(len(self.ikIndices)):
self.ikIndices[j] = j
self.refresh()
break
elif c == 't':
clicked = self.click_world(x, y)
if clicked is not None and isinstance(clicked[0], RobotModelLink):
#make a new widget
link, wpt = clicked
Tlink = link.getTransform()
self.ikIndices.append(len(self.ikWidgets))
self.ikWidgets.append(TransformPoser())
self.ikWidgets[-1].set(*Tlink)
self.widgetMaster.add(self.ikWidgets[-1])
self.ikProblem.addObjective(
ik.objective(link, R=Tlink[0], t=Tlink[1]))
self.refresh()
elif c == 'f':
self.ikdb.flush()
elif c == 's':
self.ikdb.save()
elif c == 'b':
self.ikdb.backgroundStep()
self.refresh()
elif c == 'B':
if hasattr(self.ikdb, 'thread'):
self.ikdb.stopBackgroundLoop()
else:
self.ikdb.startBackgroundLoop(0)
elif c == 'v':
self.drawDb = not self.drawDb
elif c == 'c':
self.continuous = not self.continuous
elif c == 'o':
self.ikProblem.setSoftObjectives(not self.ikProblem.softObjectives)
def control_loop(self):
#Calculate the desired velocity for each robot by adding up all
#commands
rvels = [[0] * self.world.robot(r).numLinks()
for r in range(self.world.numRobots())]
robot = self.world.robot(0)
ee_link = 6
ee_local_p1 = [-0.015, -0.02, 0.27]
q_curr = robot.getConfig()
q_curr[3] = -q_curr[3]
q_curr[5] = -q_curr[5]
q_output = vectorops.mul(q_curr[1:], angle_to_degree)
milestone = (0.1, {
'robot': q_output,
'gripper': [0, 0, 0],
'vaccum': [0]
})
self.output.append(milestone)
if self.start_flag == 0:
curr_position = robot.link(ee_link).getWorldPosition(ee_local)
curr_orientation, p = robot.link(ee_link).getTransform()
self.start_T = [curr_orientation, curr_position]
self.start_flag = 1
local_p = []
if self.state == 'idle':
t = 0.5
if self.score == self.numberOfObjects:
print 'finished!'
f = open('test.json', 'w')
json.dump(self.output, f)
f.close()
exit()
else:
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
self.end_T = [[0, 0, -1, 0, 1, 0, 1, 0, 0], idle_position]
# self.idle_T=self.end_T
# self.sim.controller(0).setPIDCommand(self.idleq,[0]*7)
[local_p, world_p] = interpolate(self.start_T, self.end_T,
u, 1)
# constraints=ik.objective(robot.link(ee_link),local=local_p,world=world_p)
# traj1 = cartesian_trajectory.cartesian_interpolate_linear(robot,self.start_T,self.end_T,constraints,delta=1e-2,maximize=False)
# print 'traj1:',traj1[0]
else:
# print 'idle', robot.getConfig()
self.set_state('find_target')
elif self.state == 'find_target':
h = 0
for i in self.waiting_list:
if self.sim.world.rigidObject(i).getTransform()[1][2] > h:
self.target = i
h = self.sim.world.rigidObject(i).getTransform()[1][2]
print 'target is ', self.target
# self.target=0
# self.object_p=self.sim.world.rigidObject(self.target).getTransform()[1]
bb1, bb2 = self.sim.world.rigidObject(
self.target).geometry().getBB()
self.object_p = vectorops.div(vectorops.add(bb1, bb2), 2)
# print 'object xform', self.sim.world.rigidObject(self.target).getTransform()[1]
# print 'object_p',self.object_p
h = self.object_p[2] + box_vacuum_offset[2]
if self.object_p[1] > shelf_position[1]:
end_p = approach_p1
end_p[2] = h
else:
end_p = approach_p2
end_p[2] = h
d = vectorops.distance(end_p[:1], self.object_p[:1])
dx = self.object_p[0] - end_p[0]
dy = self.object_p[1] - end_p[1]
self.end_T = [[0, 0, -1, -dy / d, dx / d, 0, dx / d, dy / d, 0],
end_p]
print 'approach start position', end_p
self.retract_T = [[
0, 0, -1, -dy / d, dx / d, 0, dx / d, dy / d, 0
], end_p]
print 'retract_T', self.retract_T
self.set_state('preposition')
elif self.state == 'preposition':
t = 0.5
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
1)
else:
self.end_T[1] = vectorops.add(self.object_p, [0.10, 0, 0.12])
self.set_state('pregrasp')
elif self.state == 'pregrasp':
t = 0.5
# print 'pregrasp'
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
1)
else:
self.end_T[1][2] -= 0.03
self.set_state('grasp')
elif self.state == 'grasp':
# print 'grasp'
t = 0.5
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
1)
else:
self.end_T[1][2] += 0.03
self.set_state('prograsp')
elif self.state == 'prograsp':
# print 'grasp'
self.graspedObjects[self.target] = True
t = 0.5
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
1)
else:
self.end_T = self.retract_T
self.end_T[1][2] += 0.01
self.set_state('retract')
elif self.state == 'retract':
# print 'retract'
t = 0.5
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
1)
else:
self.set_state('go_to_tote')
elif self.state == 'go_to_tote':
x = robot.link(ee_link).getTransform()[1][0]
if x > (order_box_min[0] + order_box_max[0]) / 2:
q = robot.getConfig()
q[1] += 0.02
# self.sim.controller(0).setPIDCommand(q,[0]*7)
robotController.setLinear(q, 0.001)
# self.output.append(vectorops.mul(q[1:],angle_to_degree))
else:
bb1, bb2 = self.sim.world.rigidObject(
self.target).geometry().getBB()
bbx = bb2[0] - bb1[0]
bby = bb2[1] - bb1[1]
print 'BB:', bbx, bby
# self.end_T[1]=[order_box_min[0]+bbx/2-0.01,order_box_min[1]+bby/2+0.21-self.target*0.1,0.8]
self.end_T[1] = self.find_placement()
# if self.score>0:
# pass
self.end_T[0] = [0, 0, -1, -1, 0, 0, 0, 1, 0]
self.set_state('place')
elif self.state == 'place':
t = 0.5
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
0)
else:
self.end_T[1][2] -= 0.01
self.set_state('release')
elif self.state == 'release':
t = 0.5
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
0)
else:
self.graspedObjects[self.target] = False
self.check_target()
self.set_state('idle')
old_T = robot.link(ee_link).getTransform()
old_R, old_t = old_T
if local_p:
q_old = robot.getConfig()
goal = ik.objective(robot.link(ee_link),
local=local_p,
world=world_p)
# s=ik.solver(goal)
# s.setMaxIters(100)
# s.setTolerance(0.2)
# if s.solve():
# pass
# else:
# print "IK failed"
# while not s.solve():
# q_restart=[0,0,0,0,0,0,0]
# q=robot.getConfig()
# (qmin,qmax) = robot.getJointLimits()
# for i in range(7):
# q_restart[i]=random.uniform(-1,1)+q[i]
# q_restart[i] = min(qmax[i],max(q_restart[i],qmin[i]))
# print q_restart
# robot.setConfig(q_restart)
# if s.solve():
# print "IK solved"
# else:
# print "IK failed"
# s=ik.solve_global(goal)
s = ik.solve_nearby(goal,
maxDeviation=10,
feasibilityCheck=test_function)
q = robot.getConfig()
if not feasible(robot, q, q_old):
s = ik.solve_global(goal)
q = robot.getConfig()
robotController = self.sim.controller(0)
qdes = q
(qmin, qmax) = robot.getJointLimits()
for i in xrange(len(qdes)):
qdes[i] = min(qmax[i], max(qdes[i], qmin[i]))
# robotController.setPIDCommand(qdes,rvels[0])
robotController.setLinear(qdes, 0.001)
# self.output.append(vectorops.mul(qdes[1:],angle_to_degree))
obj = self.sim.world.rigidObject(self.target)
#get a SimBody object
body = self.sim.body(obj)
if self.graspedObjects[self.target]:
T = body.getTransform()
R, t = T
body.enableDynamics(False)
if self.flag == 0:
self.flag = 1
self.t = robot.link(ee_link).getLocalPosition(t)
# print 'here'
new_T = robot.link(ee_link).getTransform()
new_R, new_t = new_T
change_R = so3.mul(new_R, so3.inv(old_R))
R = so3.mul(change_R, R)
body.setTransform(R, robot.link(ee_link).getWorldPosition(self.t))
else:
body.enableDynamics(True)
self.flag = 0
return
if not MANUAL_SPACE_CREATION:
space = robotplanning.makeSpace(world=world,
robot=robot,
edgeCheckResolution=1e-3,
movingSubset='all')
else:
#Manual construction of space
collider = WorldCollider(world)
space = robotcspace.RobotCSpace(robot, collider)
space.eps = 1e-3
space.setup()
else:
#TESTING: closed loop robot cspace
collider = WorldCollider(world)
obj = ik.objective(robot.link(robot.numLinks() - 1),
local=[0, 0, 0],
world=[0.5, 0, 0.5])
vis.add("IK goal", obj)
vis.dialog()
space = robotcspace.ClosedLoopRobotCSpace(robot, obj, collider)
space.eps = 1e-3
space.setup()
#Generate some waypoint configurations using the resource editor
print("#########################################")
print("Editing the waypoints")
print("#########################################")
configs = resource.get("planningtest.configs",
"Configs",
default=[],
world=world,
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)
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 keyboardfunc(self,c,x,y):
if c=='h':
print 'HELP:'
print '[right-click]: add a new IK constraint'
print '[space]: tests the current configuration'
print 'd: deletes IK constraint'
print 't: adds a new rotation-fixed IK constraint'
print 'f: flushes the current database to disk'
print 's: saves the current database to disk'
print 'b: performs one background step'
print 'B: starts / stops the background thread'
print 'v: toggles display of the database'
print 'c: toggles continuous re-solving of IK constraint its as being moved'
print 'o: toggles soft / hard IK constraints'
elif c==' ':
self.planningWorld.robot(0).setConfig(self.currentConfig)
soln = self.ikdb.solve(self.ikProblem)
if soln:
print "Solved"
self.currentConfig = soln
self.refresh()
else:
print "Failure"
elif c=='d':
for i,w in enumerate(self.ikWidgets):
if w.hasHighlight():
print "Deleting IK widget"
#delete it
index = self.ikIndices[i]
self.widgetMaster.remove(w)
del self.ikWidgets[i]
del self.ikIndices[i]
del self.ikProblem.objectives[index]
for j in range(len(self.ikIndices)):
self.ikIndices[j] = j
self.refresh()
break
elif c=='t':
clicked = self.click_world(x,y)
if clicked is not None and isinstance(clicked[0],RobotModelLink):
#make a new widget
link, wpt = clicked
Tlink = link.getTransform()
self.ikIndices.append(len(self.ikWidgets))
self.ikWidgets.append(TransformPoser())
self.ikWidgets[-1].set(*Tlink)
self.widgetMaster.add(self.ikWidgets[-1])
self.ikProblem.addObjective(ik.objective(link,R=Tlink[0],t=Tlink[1]))
self.refresh()
elif c=='f':
self.ikdb.flush()
elif c=='s':
self.ikdb.save()
elif c=='b':
self.ikdb.backgroundStep()
self.refresh()
elif c=='B':
if hasattr(self.ikdb,'thread'):
self.ikdb.stopBackgroundLoop()
else:
self.ikdb.startBackgroundLoop(0)
elif c=='v':
self.drawDb = not self.drawDb
elif c=='c':
self.continuous = not self.continuous
elif c == 'o':
self.ikProblem.setSoftObjectives(not self.ikProblem.softObjectives)
def modification_template(world):
"""Tests a variety of miscellaneous vis functions"""
vis.add("world",world)
robot = world.robot(0)
vis.setColor(("world",world.terrain(0).getName()),1,0,0,0.5) #turn the terrain red and 50% opaque
import random
for i in range(10):
#set some links to random colors
randlink = random.randint(0,robot.numLinks()-1)
color = (random.random(),random.random(),random.random())
vis.setColor(("world",robot.getName(),robot.link(randlink).getName()),*color)
#test the on-screen text display
vis.addText("text2","Here's some red text")
vis.setColor("text2",1,0,0)
vis.addText("text3","Here's bigger text")
vis.setAttribute("text3","size",24)
vis.addText("text4","Transform status")
vis.addText("textbottom","Text anchored to bottom of screen",(20,-30))
#test a point
pt = [2,5,1]
vis.add("some point",pt)
#test a rigid transform
vis.add("some blinking transform",[so3.identity(),[1,3,0.5]])
vis.edit("some point")
#vis.edit("some blinking transform")
#vis.edit("coordinates:ATHLETE:ankle roll 3")
#test an IKObjective
link = world.robot(0).link(world.robot(0).numLinks()-1)
#point constraint
obj = ik.objective(link,local=[[0,0,0]],world=[pt])
#hinge constraint
#obj = ik.objective(link,local=[[0,0,0],[0,0,0.1]],world=[pt,[pt[0],pt[1],pt[2]+0.1]])
#transform constraint
#obj = ik.objective(link,R=link.getTransform()[0],t=pt)
vis.add("ik objective",obj)
#enable plotting
vis.addPlot('plot')
vis.addPlotItem('plot','some point')
vis.setPlotDuration('plot',10.0)
#run the visualizer, which runs in a separate thread
vis.setWindowTitle("Manual animation visualization test")
class MyCallback:
def __init__(self):
self.iteration = 0
def __call__(self):
vis.lock()
#TODO: you may modify the world here. This line tests a sin wave.
pt[2] = 1 + math.sin(self.iteration*0.03)
vis.unlock()
#changes to the visualization with vis.X functions can done outside the lock
if (self.iteration % 100) == 0:
if (self.iteration / 100)%2 == 0:
vis.hide("some blinking transform")
vis.addText("text4","The transform was hidden")
vis.logPlotEvent('plot','hide')
else:
vis.hide("some blinking transform",False)
vis.addText("text4","The transform was shown")
vis.logPlotEvent('plot','show')
#this is another way of changing the point's data without needing a lock/unlock
#vis.add("some point",[2,5,1 + math.sin(iteration*0.03)],keepAppearance=True)
#or
#vis.setItemConfig("some point",[2,5,1 + math.sin(iteration*0.03)])
if self.iteration == 200:
vis.addText("text2","Going to hide the text for a second...")
if self.iteration == 400:
#use this to remove text
vis.clearText()
if self.iteration == 500:
vis.addText("text2","Text added back again")
vis.setColor("text2",1,0,0)
self.iteration += 1
callback = MyCallback()
if not MULTITHREADED:
vis.loop(callback=callback,setup=vis.show)
else:
vis.show()
while vis.shown():
callback()
time.sleep(0.01)
#use this to remove a plot
vis.remove("plot")
vis.kill()
def add_milestones(test_cspace, robot, milestones, t, control_rate, start_T,
end_T, vacuum_status, simulation_status, ik_indicator):
if t < 0.3:
t = 0.3
steps = t * control_rate
t_step = 1.0 / control_rate
# print "start config",robot.getConfig()
i = 0
start_q = robot.getConfig()[3]
while i <= steps:
q_old = robot.getConfig()
u = i * 1.0 / steps
# print u
[local_p, world_p] = interpolate(start_T, end_T, u, ik_indicator)
goal = ik.objective(robot.link(ee_link), local=local_p, world=world_p)
s = ik.solve_global(goal)
# s=ik.solve_nearby(goal,maxDeviation=1000,feasibilityCheck=test_function)
q = robot.getConfig()
n = 0
# print i
# print s
# print test_cspace.feasible(q)
flag = 1
if (max(vectorops.sub(q_old, q)) >
max_change) or (min(vectorops.sub(q_old, q)) <
(-max_change)) or q[3] * start_q < 0:
print "too much change!"
print vectorops.sub(q_old, q)
flag = 0
while n < 30:
if flag and (s and test_cspace.feasible(q)):
break
else:
# print "no feasible ik solution found"
# print world_p
robot.setConfig(q_old)
s = ik.solve_global(goal)
# s=ik.solve_nearby(goal,maxDeviation=1000,feasibilityCheck=test_function)
q = robot.getConfig()
if (max(vectorops.sub(q_old, q)) >
max_change) or (min(vectorops.sub(q_old, q)) <
(-max_change)) or q[3] * start_q < 0:
print "too much change!"
flag = 0
else:
flag = 1
# print 's',s
# print 'feasible test:',test_cspace.feasible(q)
n += 1
if flag and s and test_cspace.feasible(q):
m_change = max(max(vectorops.sub(q_old, q)),
-min(vectorops.sub(q_old, q)))
milestones.append(
make_milestone(t_step, q, vacuum_status, simulation_status))
i += 1
else:
print 'no feasible solution can be found!!!!!'
print s
return False
return milestones
