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 plan_pick_one(world,robot,object,gripper,grasp):
"""
Plans a picking motion for a given object and a specified grasp.
Arguments:
world (WorldModel): the world, containing robot, object, and other items that
will need to be avoided.
robot (RobotModel): the robot in its current configuration
object (RigidObjectModel): the object to pick.
gripper (GripperInfo): the gripper.
grasp (Grasp): the desired grasp. See common/grasp.py for more information.
Returns:
None or (transit,approach,lift): giving the components of the pick motion.
Each element is a RobotTrajectory. (Note: to convert a list of milestones
to a RobotTrajectory, use RobotTrajectory(robot,milestones=milestones)
Tip:
vis.debug(q,world=world) will show a configuration.
"""
qstart = robot.getConfig()
grasp.ik_constraint.robot = robot #this makes it more convenient to use the ik module
#TODO solve the IK problem for qgrasp?
def collision_free():
return is_collision_free_grasp(world, robot, object)
ik.solve_global([grasp.ik_constraint], iters=100, numRestarts=10, feasibilityCheck=collision_free)
qgrasp = robot.getConfig()
qgrasp = grasp.set_finger_config(qgrasp) #open the fingers the right amount
qopen = gripper.set_finger_config(qgrasp,gripper.partway_open_config(1)) #open the fingers further
qpregrasp = retract(robot, gripper, [0,0,-0.2]) #TODO solve the retraction problem for qpregrasp?
if qpregrasp is None:
return None
qstartopen = gripper.set_finger_config(qstart,gripper.partway_open_config(1)) #open the fingers of the start to match qpregrasp
robot.setConfig(qstartopen)
# print(qpregrasp)
transit = optimizing_plan(world,robot,qpregrasp) #decide whether to use feasible_plan or optimizing_plan
if not transit:
return None
#TODO: not a lot of collision checking going on either...
robot.setConfig(qgrasp)
qlift = retract(robot, gripper, [0,0,-0.2])
return (RobotTrajectory(robot,milestones=[qstart]+transit),RobotTrajectory(robot,milestones=[qpregrasp,qopen,qgrasp]),RobotTrajectory(robot,milestones=[qgrasp,qlift]))
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 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 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 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 plan_pick_one(world,robot,object,gripper,grasp, robot0=True):
"""
Plans a picking motion for a given object and a specified grasp.
Arguments:
world (WorldModel): the world, containing robot, object, and other items that
will need to be avoided.
robot (RobotModel): the robot in its current configuration
object (RigidObjectModel): the object to pick.
gripper (GripperInfo): the gripper.
grasp (Grasp): the desired grasp. See common/grasp.py for more information.
Returns:
None or (transit,approach,lift): giving the components of the pick motion.
Each element is a RobotTrajectory. (Note: to convert a list of milestones
to a RobotTrajectory, use RobotTrajectory(robot,milestones=milestones)
Tip:
vis.debug(q,world=world) will show a configuration.
"""
qstart = robot.getConfig()
qstartopen = gripper.set_finger_config(qstart, gripper.partway_open_config(1)) # open the fingers of the start to match qpregrasp
robot.setConfig(qstartopen)
grasp.ik_constraint.robot = robot #this makes it more convenient to use the ik module
def feasible():
return is_collision_free_grasp(world, robot, object)
# return True
solved = ik.solve_global(objectives=grasp.ik_constraint, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7],feasibilityCheck=feasible)
print('ik status:', solved)
if not solved: return None
qpregrasp = robot.getConfig()
robot.setConfig(qpregrasp)
if not feasible(): return None
qtransit = retract(robot=robot, gripper=gripper, amount=list(-0.1*np.array(gripper.primary_axis)), local=True)
# rotate the gripper along its primary axis to make its secondary axis perpendicular to the object
# so that it can grasp the object easily
secondary_axis_gripper = gripper.secondary_axis
if not isinstance(gripper,(int,str)):
gripper1 = gripper.base_link
else:
gripper1 = gripper
link = robot.link(gripper1)
R_gripper_w, _ = link.getTransform()
secondary_axis_world = so3.apply(R_gripper_w, secondary_axis_gripper)
secondary_axis_world_2d = np.array(secondary_axis_world)[:-1]
angle = np.arccos(np.dot(secondary_axis_world_2d, [0, 1]))
q_rotate = copy.deepcopy(qtransit)
q_rotate[7] = clip_angle(q_rotate[7] - angle)
qpregrasp[7] = clip_angle(qpregrasp[7] - angle)
if qtransit is None: return None
print(qtransit)
robot.setConfig(qtransit)
if not feasible(): return None
robot.setConfig(qpregrasp)
qopen = gripper.set_finger_config(qpregrasp, gripper.partway_open_config(1)) # open the fingers further
robot.setConfig(qopen)
if not feasible(): return None
if robot0:
close_amount = 0.1
else:
close_amount = 0.8
qgrasp = gripper.set_finger_config(qopen, gripper.partway_open_config(close_amount)) # close the gripper
robot.setConfig(qgrasp)
qlift = retract(robot=robot, gripper=gripper, amount=[0, 0, 0.1], local=False)
robot.setConfig(qlift)
if not feasible(): return None
robot.setConfig(qstart)
transit = feasible_plan(world, robot, object, qtransit) # decide whether to use feasible_plan or optimizing_plan
if not transit:
return None
return RobotTrajectory(robot,milestones=[qstart]+transit),\
RobotTrajectory(robot,milestones=[qtransit, q_rotate, qpregrasp, qopen, qgrasp]),\
RobotTrajectory(robot,milestones=[qgrasp,qlift])
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
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
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 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())]
# print rvels
# print self.world.robot(0).link(7).getWorldPosition([0,0,0])
#send to the robot(s)
# self.target=vectorops.add(self.world.rigidObject(self.world.numRigidObjects()-1).getTransform()[1],[-0.05,0,0.1])
robot=self.world.robot(0)
ee_link=6
ee_local_p1=[0,0,0]
ee_local_p2=[0.01,0,0]
ee_local_p3=[0,0,0.01]
ee_world_p1=self.target
ee_world_p2=vectorops.add(self.target,[0,0,-0.01])
ee_world_p3=vectorops.add(self.target,[0.01,0,0])
goal = ik.objective(robot.link(ee_link),local=ee_local_p1,world=ee_world_p1)
# goal = ik.objective(robot.link(ee_link),local=[ee_local_p1,ee_local_p2,ee_local_p3],world=[ee_world_p1,ee_world_p2,ee_world_p3])
old_T=robot.link(ee_link).getTransform()
old_R,old_t=old_T
# s=ik.solver(goal)
# s.setMaxIters(100)
# s.setTolerance(0.001)
# s.solve()
s=ik.solve_global(goal)
q= robot.getConfig()
print self.cspace.feasible(q)
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.setLinear(qdes,0.1)
obj = self.sim.world.rigidObject(self.world.numRigidObjects()-1)
#get a SimBody object
body = self.sim.body(obj)
if self.activeObjects:
t1=robot.link(ee_link).getWorldPosition([0,0,0.1])
t2=body.getTransform()[1]
t=vectorops.sub(t1,t2)
n=vectorops.norm(t)
body.applyForceAtPoint(vectorops.mul(vectorops.div(t,n),40),t1)
# T=body.getTransform()
# R,t=T
# body.enableDynamics(False)
# if self.flag==0:
# self.flag=1
# self.t=robot.link(ee_link).getLocalPosition(t)
# 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
