def gen_est_config_list(robot, Clink_set, origin_config):
"""
:param robot: {Klampt.RobotModel} -- the robot model
:param Clink_set: {list} -- the strandard robot configuration list
:param origin_config: {list} -- the actual original start configuration
:return: est_config_list: {list} -- the estimated configuration list
"""
est_config_list = []
est_config_list.append(origin_config)
robot.setConfig(origin_config)
origin_trans = robot.link('link_6').getTransform()
robot.setConfig(Clink_set[0])
mother_trans = robot.link('link_6').getTransform()
diff_trans = se3.mul(origin_trans, se3.inv(mother_trans))
local_pos = [[1, 0.0, 0.0], [0.0, 1, 0.0], [0.0, 0.0, 1]]
for i in range(1, len(Clink_set)):
robot.setConfig(Clink_set[i])
child_trans = se3.mul(diff_trans, robot.link('link_6').getTransform())
relative_trans = se3.mul(child_trans, se3.inv(origin_trans))
robot.setConfig(origin_config)
world_pos = [
se3.apply(relative_trans,
robot.link('link_6').getWorldPosition(local_pos[0])),
se3.apply(relative_trans,
robot.link('link_6').getWorldPosition(local_pos[1])),
se3.apply(relative_trans,
robot.link('link_6').getWorldPosition(local_pos[2]))
]
q = solve_ik(robot.link('link_6'), local_pos, world_pos, i)
est_config_list.append(q)
return est_config_list
def opt_error_fun_penalize(est_input, *args):
"""
:param est_input: {numpy.array} -- the initial guess of the transformation of Toct and Tneedle
:param args: {tuple} -- the set of tranfermation of Tlink and ICP transformation matrix
:return: error: {float} -- the error between the true transformation and estimated transformation
"""
est_Toct = (so3.from_rpy(est_input[0:3]), est_input[3:6])
est_Tneedle = (so3.from_rpy(est_input[6:9]), est_input[9:12])
Tlink_set = args[0]
Tneedle2oct_icp = args[1]
fun_list = np.array([])
for i in range(len(Tlink_set)):
Toct2needle_est = se3.mul(se3.mul(se3.inv(est_Toct), Tlink_set[i]),
est_Tneedle)
fun_list = np.append(
fun_list,
np.absolute(
np.multiply(
se3.error(Tneedle2oct_icp[i],
se3.inv(Toct2needle_est))[0:3],
[1, args[2], 1])))
fun_list = np.append(
fun_list,
np.absolute(
np.multiply(
se3.error(Tneedle2oct_icp[i],
se3.inv(Toct2needle_est))[3:6], 1000)))
return fun_list
def error_analysis(self):
"""
:return: None
"""
Toct = (so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6])
Tneedle = (so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12])
trans_error_list = []
trans_error_mat = []
rot_error_list = []
rot_error_mat = []
redraw_list = []
for i in range(len(self.Tlink_set)):
Toct_needle_est = se3.mul(
se3.mul(se3.inv(Toct), self.Tlink_set[i]), Tneedle)
trans_error = vectorops.sub(
se3.inv(self.trans_list[i])[1], Toct_needle_est[1])
rot_error = so3.error(
se3.inv(self.trans_list[i])[0], Toct_needle_est[0])
trans_error_list.append(vectorops.normSquared(trans_error))
trans_error_mat.append(np.absolute(trans_error))
rot_error_list.append(vectorops.normSquared(rot_error))
rot_error_mat.append(np.absolute(rot_error))
redraw_list.append(
redraw_pcd(self.pcd_list[i], Toct_needle_est)[0])
redraw_list.append(
redraw_pcd(self.pcd_list[i], Toct_needle_est)[1])
redraw_list.append(
create_mesh_coordinate_frame(size=0.0015, origin=[0, 0, 0]))
draw_geometries(redraw_list)
trans_error_list = np.array(trans_error_list)
trans_error_mat = np.array(trans_error_mat)
rot_error_list = np.array(rot_error_list)
rot_error_mat = np.array(rot_error_mat)
rmse_trans = np.sqrt(np.mean(trans_error_list))
rmse_rot = np.sqrt(np.mean(rot_error_list))
print("The squared translation error list is:\n ",
np.sqrt(trans_error_list), "\nAnd the its RMSE is ", rmse_trans)
print("The mean error in XYZ directions is: ",
np.mean(trans_error_mat, axis=0))
print("The squared rotation error list is:\n ",
np.sqrt(rot_error_list), "\nAnd the its RMSE is ", rmse_rot)
print("The mean error in three rotation vectors is: ",
np.mean(rot_error_mat, axis=0))
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_rmse.npy", np.array([rmse_trans, rmse_rot]), allow_pickle=True)
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_list.npy", np.array([np.sqrt(trans_error_list),
# np.sqrt(rot_error_list)]), allow_pickle=True)
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_mat.npy", np.array([np.mean(trans_error_mat, axis=0),
# np.mean(rot_error_mat, axis=0)]), allow_pickle=True)
trans_all2needle(self.Tlink_set, Toct, Tneedle, self.pcd_list)
def setGhostConfig(self,q,name="ghost",robot=0):
"""Sets the configuration of the ghost to q. If the ghost is named, place its name
in prefixname."""
if robot < 0 or robot >= self.world.numRobots():
raise ValueError("Invalid robot specified")
robot = self.world.robot(robot)
q_original = robot.getConfig()
if len(q) != robot.numLinks():
raise ValueError("Config must be correct size: %d != %d"%(len(q),robot.numLinks()))
robot.setConfig(q)
self.beginRpc(strict=False)
rpcs = []
for i in range(robot.numLinks()):
T = robot.link(i).getTransform()
p = robot.link(i).getParent()
if p>=0:
Tp = robot.link(p).getTransform()
T = se3.mul(se3.inv(Tp),T)
mat = se3.homogeneous(T)
#mat is now a 4x4 homogeneous matrix
linkname = name+robot.link(i).getName()
#send to the ghost link with name "name"...
self._do_rpc({'type':'set_transform','object':linkname,'matrix':[mat[0][0],mat[0][1],mat[0][2],mat[0][3],mat[1][0],mat[1][1],mat[1][2],mat[1][3],mat[2][0],mat[2][1],mat[2][2],mat[2][3],mat[3][0],mat[3][1],mat[3][2],mat[3][3]]})
self.endRpc(strict=False)
robot.setConfig(q_original) #restore original config
def addForceSpring(self,obj,worldpt):
self.sim.updateWorld()
self.forceObject = obj
Ro,to = obj.getTransform()
self.forceAnchorPoint = worldpt
self.forceLocalPoint = se3.apply(se3.inv((Ro,to)),self.forceAnchorPoint)
self.sim.addHook(obj,lambda obj:self.simulateForceSpring())
def cal_est_config_list(robot, serial_num):
est_config_list = []
local_pos = [[1, 0.0, 0.0], [0.0, 1, 0.0], [0.0, 0.0, 1]]
Ticp_list = np.load("../../data/suture_experiment/standard_trans_list/trans_array.npy")
est_calibration = np.load("../../data/suture_experiment/calibration_result_files/" +
serial_num + "B/calibration_result.npy")
Toct = (so3.from_rpy(est_calibration[0:3]), est_calibration[3:6])
Tneedle = (so3.from_rpy(est_calibration[6:9]), est_calibration[9:12])
for i in range(0, 9):
Tee = se3.mul(se3.mul(Toct, se3.inv(Ticp_list[i])), se3.inv(Tneedle))
world_pos = [se3.apply(Tee, local_pos[0]),
se3.apply(Tee, local_pos[1]),
se3.apply(Tee, local_pos[2])]
q = solve_ik(robot.link('link_6'), local_pos, world_pos, i)
est_config_list.append(q)
return est_config_list
def getJacobian(self, q):
self.robot.setConfig(q)
J = None
if self.taskType == 'po':
J = self.link.getJacobian(self.localPosition)
elif self.taskType == 'position':
J = self.link.getPositionJacobian(self.localPosition)
elif self.taskType == 'orientation':
J = self.link.getOrientationJacobian()
else:
raise ValueError("Invalid taskType "+self.taskType)
J = np.array(J)
#check if relative transform task, modify Jacobian accordingly
if self.baseLinkNo >= 0:
T = self.link.getTransform()
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
pb = se3.apply(Tbinv,se3.apply(T,self.localPosition))
if self.taskType == 'po':
Jb = self.baseLink.getJacobian(pb)
elif self.taskType == 'position':
Jb = self.baseLink.getPositionJacobian(pb)
elif self.taskType == 'orientation':
Jb = self.baseLink.getOrientationJacobian()
Jb = np.array(Jb)
#subtract out jacobian w.r.t. baseLink
J -= Jb
if self.activeDofs!=None:
J = select_cols(J,self.activeDofs)
return J
def getJacobian(self, q):
self.robot.setConfig(q)
J = None
if self.taskType == 'po':
J = self.link.getJacobian(self.localPosition)
elif self.taskType == 'position':
J = self.link.getPositionJacobian(self.localPosition)
elif self.taskType == 'orientation':
J = self.link.getOrientationJacobian()
else:
raise ValueError("Invalid taskType " + self.taskType)
J = np.array(J)
#check if relative transform task, modify Jacobian accordingly
if self.baseLinkNo >= 0:
T = self.link.getTransform()
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
pb = se3.apply(Tbinv, se3.apply(T, self.localPosition))
if self.taskType == 'po':
Jb = self.baseLink.getJacobian(pb)
elif self.taskType == 'position':
Jb = self.baseLink.getPositionJacobian(pb)
elif self.taskType == 'orientation':
Jb = self.baseLink.getOrientationJacobian()
Jb = np.array(Jb)
#subtract out jacobian w.r.t. baseLink
J -= Jb
if self.activeDofs != None:
J = select_cols(J, self.activeDofs)
return J
def display(self):
T1 = self.taskGen.getDesiredCartesianPose('left',0)
T2 = self.taskGen.getDesiredCartesianPose('right',1)
baseTransform = self.taskGen.world.robot(0).link(2).getTransform()
glEnable(GL_LIGHTING)
if T1 is not None:
gldraw.xform_widget(se3.mul(baseTransform,T1),0.2,0.03,fancy=True,lighting=True)
if T2 is not None:
gldraw.xform_widget(se3.mul(baseTransform,T2),0.2,0.03,fancy=True,lighting=True)
dstate = self.taskGen.serviceThread.hapticupdater.deviceState
if T1 is not None and dstate[0] != None:
R = dstate[0].widgetCurrentTransform[0]
T = se3.mul(baseTransform,(R,T1[1]))
self.draw_wand(T)
if T2 is not None and dstate[1] != None:
R = dstate[1].widgetCurrentTransform[0]
T = se3.mul(baseTransform,(R,T2[1]))
self.draw_wand(T)
if debugHapticTransform:
for deviceState in dstate:
#debugging
#this is the mapping from the handle to the user frame
Traw = (so3.from_moment(deviceState.rotationMoment),deviceState.position)
Tuser = se3.mul(se3.inv(handleToUser),Traw)
T = se3.mul(userToWorld,se3.mul(Tuser,worldToUser))
T = (T[0],so3.apply([0,-1,0, 0,0,1, -1,0,0],T[1]))
T = deviceToViewTransform(Traw[0],Traw[1])
gldraw.xform_widget(se3.mul(se3translation([-1,0,0]),Traw),0.5,0.05,lighting=True,fancy=True)
gldraw.xform_widget(se3.mul(se3translation([-0.5,0,0]),Tuser),0.5,0.05,lighting=True,fancy=True)
#gldraw.xform_widget(se3.mul(se3translation([-0.5,0,0]),se3.mul(Traw,worldToHandle)),0.5,0.05,lighting=True,fancy=True)
gldraw.xform_widget(T,0.5,0.05,lighting=True,fancy=True)
break
def graspTransform(robot, hand, qrobot0, Tobj0):
"""Given initial robot configuration qrobot0 and object transform Tobj0,
returns the grasp transformation Tgrasp, which produces the object transform
via the composition Tobj = Thand * Tgrasp"""
robot.setConfig(qrobot0)
Thand0 = robot.link(hand.link).getTransform()
Tgrasp = se3.mul(se3.inv(Thand0), Tobj0)
return Tgrasp
def loop_callback():
global base_xform
xform = vis.getItemConfig("base_xform")
base_xform = (xform[:9], xform[9:])
vis.lock()
base_link.setParentTransform(
*se3.mul(se3.inv(parent_xform), base_xform))
vis.unlock()
def reset():
vis.lock()
robot.setConfig(q0)
base_link.setParentTransform(
*se3.mul(se3.inv(parent_xform), base_xform0))
vis.unlock()
vis.add("base_xform", base_xform0)
vis.edit("base_xform")
vis.setItemConfig("gripper", grob.getConfig())
def SimulationPoses(o_T_h_des, w_T_h, w_T_o):
#w_T_h end-effector in word frame
#h_T_o object in end-effector frame
#w_T_o pbject in word frame
if not isinstance(o_T_h_des, tuple):
o_T_h_des = se3.from_homogeneous(o_T_h_des)
if not isinstance(w_T_h, tuple):
w_T_h = se3.from_homogeneous(w_T_h)
if not isinstance(w_T_o, tuple):
w_T_o = se3.from_homogeneous(w_T_o)
posedict = {}
posedict['desired'] = [se3.inv(o_T_h_des)]
posedict['actual'] = [se3.mul(se3.inv(w_T_h), w_T_o)]
return posedict
def getSensedValue(self, q):
"""Returns CoM position
"""
self.robot.setConfig(q)
com = self.robot.getCom()
if self.baseLinkNo >= 0:
Tb = self.robot.link(self.baseLinkNo).getTransform()
Tbinv = se3.inv(Tb)
com = se3.apply(Tbinv, com)
return com
def getSensedValue(self, q): """Returns CoM position """ self.robot.setConfig(q) com = self.robot.getCom() if self.baseLinkNo >= 0: Tb = self.robot.link(self.baseLinkNo).getTransform() Tbinv = se3.inv(Tb) com = se3.apply(Tbinv,com) return com
def bumpTrajectory():
relative_xforms = []
robot.setConfig(self.refConfig)
for e in self.endeffectors:
xform = vis.getItemConfig("ee_"+robot.link(e).getName())
T = (xform[:9],xform[9:])
T0 = robot.link(e).getTransform()
Trel = se3.mul(se3.inv(T0),T)
print "Relative transform of",e,"is",Trel
relative_xforms.append(Trel)
bumpTraj = cartesian_trajectory.cartesian_bump(self.robot,self.traj,self.constraints,relative_xforms,ee_relative=True,closest=True)
assert bumpTraj != None
vis.animate(("world",world.robot(0).getName()),bumpTraj)
self.refresh()
def trans_all2needle(Tlink_set, Toct, Tneedle, pcd_list):
"""
:param Tlink_set: {list} -- the robot configuration list
:param Toct: {tuple} -- the transformation matrix from world to OCT
:param Tneedle: {tuple} -- the transformation matrix from robot end effector to needle
:param pcd_list: {list} -- the point cloud list
:return: None
"""
pcd_needle_list = []
pcd_t = copy.deepcopy(pcd_list[0][1])
pcd_t.paint_uniform_color([0, 0.651, 0.929])
pcd_needle_list.append(pcd_t)
mesh_frame = create_mesh_coordinate_frame(0.0006, [0, 0, 0])
pcd_needle_list.append(mesh_frame)
for i in range(len(Tlink_set)):
Tneedle2oct = se3.mul(se3.inv(Tneedle),
se3.mul(se3.inv(Tlink_set[i]), Toct))
pcd_s_copy = copy.deepcopy(pcd_list[i][0])
pcd_s_copy.transform(se3.homogeneous(Tneedle2oct))
pcd_s_copy.paint_uniform_color([0.4 + i * 0.1, 0.706, 0])
pcd_needle_list.append(pcd_s_copy)
draw_geometries(pcd_needle_list)
def get_geometry(self,robot,qfinger=None,type='Group'):
"""Returns a Geometry of the gripper frozen at its configuration.
If qfinger = None, the current configuration is used. Otherwise,
qfinger is a finger configuration.
type can be 'Group' (most general and fastest) or 'TriangleMesh'
(compatible with Jupyter notebook visualizer.)
"""
if qfinger is not None:
q0 = robot.getConfig()
robot.setConfig(self.set_finger_config(q0,qfinger))
res = Geometry3D()
gripper_links = self.gripper_links if self.gripper_links is not None else [self.base_link] + self.descendant_links(robot)
if type == 'Group':
res.setGroup()
Tbase = robot.link(self.base_link).getTransform()
for i,link in enumerate(gripper_links):
Trel = se3.mul(se3.inv(Tbase),robot.link(link).getTransform())
g = robot.link(link).geometry().clone()
if not g.empty():
g.setCurrentTransform(*se3.identity())
g.transform(*Trel)
else:
print("Uh... link",robot.link(link).getName(),"has empty geometry?")
res.setElement(i,g)
if qfinger is not None:
robot.setConfig(q0)
return res
else:
import numpy as np
from klampt.io import numpy_convert
#merge the gripper parts into a static geometry
verts = []
tris = []
nverts = 0
for i,link in enumerate(gripper_links):
xform,(iverts,itris) = numpy_convert.to_numpy(robot.link(link).geometry())
verts.append(np.dot(np.hstack((iverts,np.ones((len(iverts),1)))),xform.T)[:,:3])
tris.append(itris+nverts)
nverts += len(iverts)
verts = np.vstack(verts)
tris = np.vstack(tris)
for t in tris:
assert all(v >= 0 and v < len(verts) for v in t)
mesh = numpy_convert.from_numpy((verts,tris),'TriangleMesh')
res.setTriangleMesh(mesh)
if qfinger is not None:
robot.setConfig(q0)
return res
def getJacobian(self, q): """Returns axis-weighted CoM Jacobian by averaging mass-weighted Jacobian of each link. """ self.robot.setConfig(q) numLinks = self.robot.numLinks() Jcom = np.array(self.robot.getComJacobian()) #if relative positioning task, subtract out COM jacobian w.r.t. base if self.baseLinkNo >= 0: Tb = self.robot.link(self.baseLinkNo).getTransform() pb = se3.apply(se3.inv(Tb),self.robot.getCom()) Jb = np.array(self.robot.link(self.baseLinkNo).getPositionJacobian(pb)) Jcom -= Jb if self.activeDofs != None: Jcom = select_cols(Jcom,self.activeDofs) return Jcom
def getJacobian(self, q): """Returns axis-weighted CoM Jacobian by averaging mass-weighted Jacobian of each link. """ self.robot.setConfig(q) numLinks = self.robot.numLinks() Jcom = np.array(self.robot.getComJacobian()) #if relative positioning task, subtract out COM jacobian w.r.t. base if self.baseLinkNo >= 0: Tb = self.robot.link(self.baseLinkNo).getTransform() pb = se3.apply(se3.inv(Tb),self.robot.getCom()) Jb = np.array(self.robot.link(self.baseLinkNo).getPositionJacobian(pb)) Jcom -= Jb if self.activeDofs != None: Jcom = select_cols(Jcom,self.activeDofs) return Jcom
def statesEstimation(self, each_blob, previous_estimate,
previous_covariance):
'''
input: previous_estimate(np.matrix)
output: states and covariance estimated by kalman filter
'''
pre_pos_w = previous_estimate[:3, 0].T.tolist()[0]
previous_p_c = se3.apply(se3.inv(self.Tsensor), pre_pos_w)
cur_obs = np.matrix(
[each_blob.x, each_blob.y, each_blob.w, each_blob.h]).T
cur_H = self.calJaccobian(previous_p_c, BALL_RADIUS)
cur_J = self.hTransfer(previous_p_c, BALL_RADIUS, 160,
120) - cur_H * previous_estimate
updated_states, cur_cov = kalman_filter_update(
previous_estimate, previous_covariance, self.F, self.g,
self.dyn_noise, cur_H, cur_J, self.obs_noise, cur_obs)
cur_states = updated_states.T.tolist()[0]
return cur_states, cur_cov
def setGhostConfig(self, q, name="ghost", robot=0):
"""Sets the configuration of the ghost to q. If the ghost is named, place its name
in prefixname."""
if robot < 0 or robot >= self.world.numRobots():
raise ValueError("Invalid robot specified")
robot = self.world.robot(robot)
q_original = robot.getConfig()
if len(q) != robot.numLinks():
raise ValueError("Config must be correct size: %d != %d" %
(len(q), robot.numLinks()))
robot.setConfig(q)
self.beginRpc(strict=False)
rpcs = []
for i in range(robot.numLinks()):
T = robot.link(i).getTransform()
p = robot.link(i).getParent()
if p >= 0:
Tp = robot.link(p).getTransform()
T = se3.mul(se3.inv(Tp), T)
mat = se3.homogeneous(T)
#mat is now a 4x4 homogeneous matrix
linkname = name + robot.link(i).getName()
#send to the ghost link with name "name"...
self._do_rpc({
'type':
'set_transform',
'object':
linkname,
'matrix': [
mat[0][0], mat[0][1], mat[0][2], mat[0][3], mat[1][0],
mat[1][1], mat[1][2], mat[1][3], mat[2][0], mat[2][1],
mat[2][2], mat[2][3], mat[3][0], mat[3][1], mat[3][2],
mat[3][3]
]
})
self.endRpc(strict=False)
robot.setConfig(q_original) #restore original config
def getSensedValue(self, q):
"""Get link x, which is rotation matrix and/or translation
"""
self.robot.setConfig(q)
T = self.link.getTransform()
#check if relative transform task, modify T to local transform
if self.baseLinkNo >= 0:
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
T = se3.mul(Tbinv,T)
if self.taskType == 'po':
x = (T[0],se3.apply(T,self.localPosition))
elif self.taskType == 'position':
x = se3.apply(T,self.localPosition)
elif self.taskType == 'orientation':
x = T[0]
else:
raise ValueError("Invalid taskType "+self.taskType)
return x
def getSensedValue(self, q):
"""Get link x, which is rotation matrix and/or translation
"""
self.robot.setConfig(q)
T = self.link.getTransform()
#check if relative transform task, modify T to local transform
if self.baseLinkNo >= 0:
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
T = se3.mul(Tbinv, T)
if self.taskType == 'po':
x = (T[0], se3.apply(T, self.localPosition))
elif self.taskType == 'position':
x = se3.apply(T, self.localPosition)
elif self.taskType == 'orientation':
x = T[0]
else:
raise ValueError("Invalid taskType " + self.taskType)
return x
def loop_callback():
global was_grasping, Tobject_grasp, solved_trajectory, trajectory_is_transfer, executing_plan, execute_start_time, qstart, next_item_to_pick
if not executing_plan:
return
if PHYSICS_SIMULATION:
execute_start_time += sim_dt
t = execute_start_time
else:
t = time.time() - execute_start_time
vis.addText("time", "Time %.3f" % (t), position=(10, 10))
if PROBLEM == '3c':
qstart = solved_trajectory.eval(t)
if PHYSICS_SIMULATION:
#sim.controller(0).setPIDCommand(qstart,solved_trajectory.deriv(t))
#sim.controller(0).setMilestone(qstart)
sim.controller(0).setLinear(qstart, sim_dt * 5)
sim.simulate(sim_dt)
sim.updateWorld()
else:
robot.setConfig(qstart)
during_transfer = trajectory_is_transfer.eval(t)[0]
if not was_grasping:
#pick up object
Tobject_grasp = se3.mul(
se3.inv(gripper_link.getTransform()),
obj.getTransform())
was_grasping = True
if during_transfer:
obj.setTransform(
*se3.mul(robot.link(9).getTransform(), Tobject_grasp))
else:
was_grasping = False
if t > solved_trajectory.duration():
executing_plan = False
solved_trajectory = None
next_item_to_pick += 1
else:
robot.setConfig(solved_trajectory.eval(t, 'loop'))
obj.setTransform(
*se3.mul(robot.link(9).getTransform(), Tobject_grasp))
def getObjectPhalanxMeanContactPoint(sim, obj, robot, links=None):
"""
Returns a contact point for each link in the robot, which is the simple arithmetic mean of all contact points,
expressed in the object frame of reference
This is not a contact centroid.
Also returns the total force applied by the link to the object, and the total moment applied to the object
(with reference pole in the object origin), expressed in the object frame of reference.
:param obj: object to grasp
:param robot: robot grasping the object
:param links: the links to check for collision
:return: (cps_avg, wrench_avg) where cps_avg is a vector 3*n_links, and wrench_avg is a vector 6*n_links
"""
oId = obj.getID()
lIds = []
_lIds = [
] # if links is not None, this contains the set of link Ids which should not be checked
lId_to_lIndex = {}
lId_to_i = {}
w_T_o = obj.getTransform()
o_T_w = se3.inv(w_T_o)
# let's first create the map from lId to lIndex, with all links
links_to_check = range(robot.numLinks())
for l_ind in links_to_check:
link = robot.link(l_ind)
lId_to_lIndex[link.getID()] = l_ind
if links is not None and l_ind not in links:
_lIds.append(link.getID())
if links is not None:
links_to_check = links
for i, l_ind in enumerate(links_to_check):
link = robot.link(l_ind)
lIds.append(link.getID())
lId_to_i[link.getID()] = i
cps_avg = np.array([float('nan')] * 3 * len(lIds))
wrench_avg = np.array([float('nan')] * 6 * len(lIds))
for lId in lIds:
clist = sim.getContacts(oId, lId)
pavg = [0, 0, 0]
navg = [0, 0, 0]
for c in clist:
pavg = vectorops.add(pavg, c[0:3])
navg = vectorops.add(navg, c[3:6])
if len(clist) > 0:
pavg = vectorops.div(pavg, len(clist))
navg = vectorops.div(navg, len(clist))
l_i = lId_to_i[lId]
cps_avg[l_i * 3:l_i * 3 + 3] = se3.apply(o_T_w, pavg)
w_F = sim.contactForce(oId, lId) # total force applied on object
w_M_obj = sim.contactTorque(
oId, lId) # total moment applied on object about it's origin
wrench_avg[l_i * 6:l_i * 6 + 3] = se3.apply_rotation(o_T_w, w_F)
wrench_avg[l_i * 6 + 3:l_i * 6 + 6] = se3.apply_rotation(
o_T_w, w_M_obj)
""" here I should first determine a "contact" reference frame
cp_M_cp = TODO
if np.all(wrench_avg[l_i*3+3:l_i*3+5] > 1e-12):
print "\n\n\n\n\n"
print "xxxxxxxxxxxxxxxxxxxxxxxxx"
print "xxxxxxxxxxxxxxxxxxxxxxxxx"
print "WARNING: moments on cp for link", robot.link(lId_to_lIndex[lId]).getName(), "are not soft finger model"
print "xxxxxxxxxxxxxxxxxxxxxxxxx"
print "xxxxxxxxxxxxxxxxxxxxxxxxx"
print "\n\n\n\n\n"
"""
for lId in _lIds:
clist = sim.getContacts(oId, lId)
if len(clist) > 0:
print "\n\n\n\n\n"
print "xxxxxxxxxxxxxxxxxxxxxxxxx"
print "xxxxxxxxxxxxxxxxxxxxxxxxx"
print "ERROR: link", robot.link(lId_to_lIndex[lId]).getName(
), "is in contact with", obj.getName(
), "but is not checked for collision"
print "xxxxxxxxxxxxxxxxxxxxxxxxx"
print "xxxxxxxxxxxxxxxxxxxxxxxxx"
print "\n\n\n\n\n"
return (cps_avg, wrench_avg)
def widgetToDeviceTransform(R, t):
"""Given a widget transform in the world coordinates, map to a device transform """
Tview = so3.rotation((0, 0, 1), math.pi), [0, 0, 0]
Rview, tview = se3.mul(se3.inv(Tview), (R, t))
return viewToDeviceTransform(Rview, tview)
#these describe the box dimensions
goal_bounds = [(-0.08, -0.68, 0.4), (0.48, -0.32, 0.5)]
#you can play around with which object you select for problem 3b...
obj = world.rigidObject(0)
gripper_link = robot.link(9)
if PROBLEM == '3a':
qstart = resource.get("transfer_start.config", world=world)
Tobj = resource.get("transfer_obj_pose.xform",
world=world,
referenceObject=obj)
obj.setTransform(*Tobj)
qgoal = resource.get("transfer_goal.config", world=world)
robot.setConfig(qstart)
Tobject_grasp = se3.mul(se3.inv(gripper_link.getTransform()), Tobj)
vis.add("goal", qgoal, color=(1, 0, 0, 0.5))
robot.setConfig(qgoal)
Tobj_goal = se3.mul(robot.link(9).getTransform(), Tobject_grasp)
vis.add("Tobj_goal", Tobj_goal)
robot.setConfig(qstart)
elif PROBLEM == '3b':
#determine a grasp pose via picking
orig_grasps = db.object_to_grasps[obj.getName()]
grasps = [
grasp.get_transformed(obj.getTransform()).transfer(
source_gripper, target_gripper) for grasp in orig_grasps
]
res = pick.plan_pick_grasps(world, robot, obj, target_gripper, grasps)
if res is None:
raise RuntimeError("Couldn't find a pick plan?")
def loop_callback():
global was_grasping, Tobject_grasp, solved_trajectory, trajectory_is_transfer, executing_plan, \
execute_start_time, qstart, next_robot_to_move, swab_time, swab_init_height, swab_height
global sensor
sensor.kinematicReset()
sensor.kinematicSimulate(world, 0.01)
if next_robot_to_move == 0:
cur_robot = robot
cur_obj = swab
elif next_robot_to_move == 1:
cur_robot = robot2
cur_obj = plate
if not executing_plan:
return
if PHYSICS_SIMULATION:
execute_start_time += sim_dt
t = execute_start_time
else:
t = time.time() - execute_start_time
vis.addText("time", "Time %.3f" % (t), position=(10, 10))
qstart = solved_trajectory.eval(t)
if PHYSICS_SIMULATION:
# sim.controller(0).setPIDCommand(qstart,solved_trajectory.deriv(t))
# sim.controller(0).setMilestone(qstart)
sim.controller(0).setLinear(qstart, sim_dt * 5)
sim.simulate(sim_dt)
sim.updateWorld()
else:
cur_robot.setConfig(qstart)
during_transfer = trajectory_is_transfer.eval(t)[0]
if not was_grasping:
# pick up object
Tobject_grasp = se3.mul(
se3.inv(cur_robot.link(9).getTransform()),
cur_obj.getTransform())
was_grasping = True
if during_transfer:
cur_obj.setTransform(
*se3.mul(cur_robot.link(9).getTransform(), Tobject_grasp))
else:
was_grasping = False
if t > solved_trajectory.duration() - 0.8:
swab_init_height = swab.getTransform()[1][2]
swab_height = copy.deepcopy(swab_init_height)
if t > solved_trajectory.duration() + 1:
executing_plan = False
solved_trajectory = None
vis.add('gripper end', cur_robot.link(9).getTransform())
next_robot_to_move += 1
# let swab drop into trash can
if solved_trajectory is not None and next_robot_to_move == 0:
if solved_trajectory.duration(
) - 0.8 < t < solved_trajectory.duration() + 1:
if swab_height > 0:
swab_height = swab_init_height - 0.5 * 1 * swab_time**2
swab_time = swab_time + sim_dt
Rs, ts = swab.getTransform()
ts[2] = swab_height
swab.setTransform(Rs, ts)
def planTriggered():
global world,robot, robot2, swab,target_gripper,grasp_swab, grasp_plate, solved_trajectory, \
trajectory_is_transfer, next_robot_to_move
########################## Plan for robot0 & swab ############################
if next_robot_to_move == 0:
# plan pick
Tobj0 = swab.getTransform()
# center of tube: (0.5 0.025 0.72)
offset = 0.005
goal_bounds = [(0.615 - offset, 0.025 - offset / 4, 0.85),
(0.615 + offset, 0.025 + offset / 4, 1.0)]
qstart = robot.getConfig()
res = pick.plan_pick_one(world,
robot,
swab,
target_gripper,
grasp_swab,
robot0=True)
if res is None:
print("Unable to plan pick")
else:
print("plan pick success")
transit, approach, lift = res
# plan place
qgrasp = lift.milestones[-1]
robot.setConfig(qgrasp)
Tobj = swab.getTransform() # swab in world frame
# swab in gripper frame
Tobject_grasp = se3.mul(se3.inv(gripper_link.getTransform()),
Tobj)
place.robot0 = True
res = place.plan_place(world, robot, swab, Tobject_grasp,
target_gripper, goal_bounds, True)
robot.setConfig(qstart)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
trajectory_is_transfer = Trajectory()
trajectory_is_transfer.times.append(0)
trajectory_is_transfer.milestones.append([0])
traj = transit
traj = traj.concat(approach,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
trajectory_is_transfer.milestones.append([1])
traj = traj.concat(lift, relative=True, jumpPolicy='jump')
traj = traj.concat(transfer,
relative=True,
jumpPolicy='jump')
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([1])
trajectory_is_transfer.milestones.append([0])
solved_trajectory = traj
swab.setTransform(*Tobj0)
vis.add("traj", traj, endEffectors=[gripper_link.index])
############################# Plan for Plate & robot 2 starts here! #################################
elif next_robot_to_move == 1:
Tobj0 = plate.getTransform()
goal_bounds = [(0.7, 0.62, 0.71), (0.7, 0.62, 0.71)]
res = pick.plan_pick_one(world,
robot2,
plate,
target_gripper,
grasp_plate,
robot0=False)
if res is None:
print("Unable to plan pick")
else:
print("plan pick success")
transit, approach, lift = res
qgrasp = lift.milestones[-1]
robot2.setConfig(qgrasp)
Tobj = plate.getTransform() # plate in world frame
# plate in gripper frame
Tobject_grasp = se3.mul(se3.inv(robot2.link(9).getTransform()),
Tobj)
place.robot0 = False
res = place.plan_place(world, robot2, plate, Tobject_grasp,
target_gripper, goal_bounds, False)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
trajectory_is_transfer = Trajectory()
trajectory_is_transfer.times.append(0)
trajectory_is_transfer.milestones.append([0])
traj = transit
traj = traj.concat(approach,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
trajectory_is_transfer.milestones.append([1])
traj = traj.concat(lift, relative=True, jumpPolicy='jump')
traj = traj.concat(transfer,
relative=True,
jumpPolicy='jump')
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([1])
trajectory_is_transfer.milestones.append([0])
traj = traj.concat(retract,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
solved_trajectory = traj
plate.setTransform(*Tobj0)
vis.add("traj", traj, endEffectors=[robot2.link(9).index])
def calculation():
"""
this is the main function of calibration calculation color.
"""
joint_positions = []
data_file = open(calidata_path + 'calibration_joint_positions.txt', 'r')
for line in data_file:
line = line.rstrip()
l = [num for num in line.split(' ')]
l2 = [float(num) for num in l]
joint_positions.append(l2)
data_file.close()
#the marker transforms here are actually just points not transformations
marker_transforms = []
data_file = open(calidata_path + 'calibration_marker_transforms.txt', 'r')
for line in data_file:
line = line.rstrip()
l = [num for num in line.split(' ')]
l2 = [float(num) for num in l]
marker_transforms.append(l2)
data_file.close()
T_marker_assumed = loader.load(
'RigidTransform', calidata_path + 'assumed_marker_world_transform.txt')
T_marker_assumed = (so3.inv(T_marker_assumed[0]), T_marker_assumed[1])
world = WorldModel()
res = world.readFile("robot_model_data/ur5Blocks.xml")
if not res:
raise RuntimeError("unable to load model")
#vis.add("world",world)
robot = world.robot(0)
link = robot.link(7)
q0 = robot.getConfig()
Tcameras = []
Tlinks = []
for i in range(len(joint_positions)):
q = robot.getConfig()
q[1:7] = joint_positions[i][0:6]
robot.setConfig(q)
Tlinks.append(link.getTransform())
#vis.add("ghost"+str(i),q)
#vis.setColor("ghost"+str(i),0,1,0,0.5)
robot.setConfig(q0)
#vis.add("MARKER",se3.mul(link.getTransform(),T_marker_assumed))
#vis.setAttribute("MARKER","width",2)
for loop in range(NLOOPS if ESTIMATE_MARKER_TRANSFORM else 1):
###Calculate Tcamera in EE (only needed for transform fit..)
###Tcameras = [se3.mul(se3.inv(Tl),se3.mul(T_marker_assumed,se3.inv(Tm_c))) for (Tl,Tm_c) in zip(Tlinks,marker_transforms)]
#actually using only point fit here....
if METHOD == 'point fit':
Tcamera2 = point_fit_transform(
[Tm_c for Tm_c in marker_transforms],
[se3.mul(se3.inv(Tl), T_marker_assumed)[1] for Tl in Tlinks])
Tcamera2 = [so3.from_matrix(Tcamera2[0]), Tcamera2[1]]
Tcamera = Tcamera2
#else:
# Tcamera = transform_average(Tcameras)
#with the Tcamera from the current iteration, calculate marker points in world
Tmarkers_world = [
se3.apply(se3.mul(Tl, Tcamera), Tm_c)
for (Tl, Tm_c) in zip(Tlinks, marker_transforms)
]
#Tmarkers_link = [se3.mul(se3.inv(Tl),Tm) for Tl,Tm in zip(Tlinks,Tmarkers)]
if ESTIMATE_MARKER_TRANSFORM:
T_marker_assumed_inv = point_fit_transform(
[[0, 0, 0] for Tm_c in marker_transforms], [
se3.apply(se3.mul(Tl, Tcamera2), Tm_c)
for (Tl, Tm_c) in zip(Tlinks, marker_transforms)
])
T_marker_assumed_inv = [
so3.from_matrix(T_marker_assumed_inv[0]),
T_marker_assumed_inv[1]
]
#print T_marker_assumed_inv
T_marker_assumed = T_marker_assumed_inv
#print "New assumed marker position",T_marker_assumed
else:
pass
#print "Would want new marker transform",transform_average(Tmarkers_world)
#print "New estimated camera position",Tcamera
SSE_t = 0
for (Tm_c, Tl) in zip(marker_transforms, Tlinks):
Tm_c_est = se3.mul(se3.mul(se3.inv(Tcamera), se3.inv(Tl)),
T_marker_assumed)
SSE_t += vectorops.distanceSquared(Tm_c, Tm_c_est[1])
#print "RMSE rotation (radians)",np.sqrt(SSE_R/len(Tlinks))
print "RMSE translations (meters)", np.sqrt(SSE_t / len(Tlinks))
#Tcameras = [se3.mul(se3.inv(Tl),se3.mul(T_marker_assumed,se3.inv(Tm_c))) for (Tl,Tm_c) in zip(Tlinks,marker_transforms)]
#Tmarkers = [se3.mul(Tl,se3.mul(Tcamera,Tm_c)) for (Tl,Tm_c) in zip(Tlinks,marker_transforms)]
print "Saving to calibrated_camera_xform.txt"
cali_txt_path = calidata_path + "calibrated_camera_xform.txt"
loader.save(Tcamera, "RigidTransform", cali_txt_path)
if ESTIMATE_MARKER_TRANSFORM:
print "Saving to calibrated_marker_link_xform.txt"
loader.save(T_marker_assumed, "RigidTransform",
calidata_path + "calibrated_marker_world_xform.txt")
#SSE_R = 0
SSE_t = 0
for (Tm_c, Tl) in zip(marker_transforms, Tlinks):
Tm_c_est = se3.mul(se3.mul(se3.inv(Tcamera), se3.inv(Tl)),
T_marker_assumed)
SSE_t += vectorops.distanceSquared(Tm_c, Tm_c_est[1])
print "RMSE translations (meters)", np.sqrt(SSE_t / len(Tlinks))
def broadcast_tf(broadcaster,
klampt_obj,
frameprefix="klampt",
root="world",
stamp=0.):
"""Broadcasts Klampt frames to the ROS tf module.
Args:
broadcaster (tf.TransformBroadcaster): the tf broadcaster
klampt_obj: the object to publish. Can be WorldModel, Simulator,
RobotModel, SimRobotController, anything with a getTransform
or getCurrentTransform method, or se3 element
frameprefix (str): the name of the base frame for this object
root (str): the name of the TF world frame.
Note:
If klampt_obj is a Simulator or SimRobotController, then its
corresponding model will be updated.
"""
from klampt import WorldModel, Simulator, RobotModel, SimRobotController
if stamp == 'now':
stamp = rospy.Time.now()
elif isinstance(stamp, (int, float)):
stamp = rospy.Time(stamp)
world = None
if isinstance(klampt_obj, WorldModel):
world = klampt_obj
elif isinstance(klampt_obj, Simulator):
world = klampt_obj.model()
klampt_obj.updateModel()
if world is not None:
prefix = frameprefix
for i in range(world.numRigidObjects()):
T = world.rigidObject(i).getTransform()
q = so3.quaternion(T[0])
broadcaster.sendTransform(
T[1], (q[1], q[2], q[3], q[0]), stamp, root,
prefix + "/" + world.rigidObject(i).getName())
for i in range(world.numRobots()):
robot = world.robot(i)
rprefix = prefix + "/" + robot.getName()
broadcast_tf(broadcaster, robot, rprefix, root)
return
robot = None
if isinstance(klampt_obj, RobotModel):
robot = klampt_obj
elif isinstance(klampt_obj, SimRobotController):
robot = klampt_obj.model()
robot.setConfig(klampt_obj.getSensedConfig())
if robot is not None:
rprefix = frameprefix
for j in range(robot.numLinks()):
p = robot.link(j).getParent()
if p < 0:
T = robot.link(j).getTransform()
q = so3.quaternion(T[0])
broadcaster.sendTransform(
T[1], (q[1], q[2], q[3], q[0]), stamp, root,
rprefix + "/" + robot.link(j).getName())
else:
Tparent = se3.mul(se3.inv(robot.link(p).getTransform()),
robot.link(j).getTransform())
q = so3.quaternion(Tparent[0])
broadcaster.sendTransform(
Tparent[1], (q[1], q[2], q[3], q[0]), stamp,
rprefix + "/" + robot.link(p).getName(),
rprefix + "/" + robot.link(j).getName())
return
transform = None
if isinstance(klampt_obj, (list, tuple)):
#assume to be an SE3 element.
transform = klampt_obj
elif hasattr(klampt_obj, 'getTransform'):
transform = klampt_obj.getTransform()
elif hasattr(klampt_obj, 'getCurrentTransform'):
transform = klampt_obj.getCurrentTransform()
else:
raise ValueError("Invalid type given to broadcast_tf: ",
klampt_obj.__class__.__name__)
q = so3.quaternion(transform[0])
broadcaster.sendTransform(transform[1], (q[1], q[2], q[3], q[0]), stamp,
root, frameprefix)
return
def start(self, q, links, baseLinks=None, endEffectorPositions=None):
"""Configures the IK solver with some set of links
After start() is called, you can set up additional settings
for the IK solver by calling :meth:`setTolerance`, :meth:`setMaxIters`,
:meth:`setActiveDofs`.
Args:
q (list of floats): the robot's current configuration.
links (int, str, list of int, or list of str): the link or links
to drive.
baseLinks (int, str, list of int, or list of str, optional): If
given, uses relative positioning mode. Each drive command is
treated as relative to the given base link. A value of -1 or an
empty str indicates no base link.
endEffectorPositions (3-vector or list of 3-vectors, optional):
Local offsets for the end effector positions, relative to the
specified link. If not given, the offsets will be treated as
zero.
"""
if not hasattr(links, '__iter__'):
links = [links]
if baseLinks is not None:
if not hasattr(baseLinks, '__iter__'):
baseLinks = [baseLinks]
if len(links) != len(baseLinks):
raise ValueError(
"Links and baseLinks must have the same length")
if endEffectorPositions is None:
endEffectorPositions = [(0, 0, 0) for i in range(len(links))]
else:
assert len(endEffectorPositions) > 0
if not hasattr(endEffectorPositions[0], '__iter__'):
endEffectorPositions = [endEffectorPositions]
if len(endEffectorPositions) != len(links):
raise ValueError(
"Links and endEffectorPositions must have the same length")
self.robot.setConfig(q)
self.links = links
self.baseLinks = baseLinks
self.endEffectorOffsets = endEffectorPositions
robotLinks = [self.robot.link(l) for l in links]
self.driveTransforms = [[
l.getTransform()[0], l.getWorldPosition(ofs)
] for l, ofs in zip(robotLinks, endEffectorPositions)]
if baseLinks is None:
baseRobotLinks = [None] * len(links)
else:
baseRobotLinks = [
self.robot.link(b) if
((isinstance(b, str) and len(b) > 0) or b >= 0) else None
for b in baseLinks
]
#compute relative drive transforms
self.driveTransforms = [
list(se3.mul(se3.inv(b.getTransform()), t))
for (b, t) in zip(baseRobotLinks, self.driveTransforms)
]
self.ikGoals = [
ik.fixed_objective(l, b, local=ofs)
for (l, b,
ofs) in zip(robotLinks, baseRobotLinks, endEffectorPositions)
]
self.solver = ik.IKSolver(self.robot)
self.solver.setMaxIters(20)
for g in self.ikGoals:
self.solver.add(g)
self.driveSpeedAdjustment = 1.0
def planTriggered():
global world, robot, obj, target_gripper, solved_trajectory, trajectory_is_transfer, Tobject_grasp, obj, next_item_to_pick, qstart
if PROBLEM == '3a':
robot.setConfig(qstart)
res = place.transfer_plan(world, robot, qgoal, obj, Tobject_grasp)
if res is None:
print("Unable to plan transfer")
else:
traj = RobotTrajectory(robot, milestones=res)
vis.add("traj", traj, endEffectors=[gripper_link.index])
solved_trajectory = traj
robot.setConfig(qstart)
elif PROBLEM == '3b':
res = place.plan_place(world, robot, obj, Tobject_grasp,
target_gripper, goal_bounds)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
traj = transfer
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
traj = traj.concat(retract, relative=True, jumpPolicy='jump')
vis.add("traj", traj, endEffectors=[gripper_link.index])
solved_trajectory = traj
robot.setConfig(qstart)
else:
robot.setConfig(qstart)
obj = world.rigidObject(next_item_to_pick)
Tobj0 = obj.getTransform()
print("STARTING TO PICK OBJECT", obj.getName())
orig_grasps = db.object_to_grasps[obj.getName()]
grasps = [
grasp.get_transformed(obj.getTransform()).transfer(
source_gripper, target_gripper) for grasp in orig_grasps
]
res = pick.plan_pick_multistep(world, robot, obj, target_gripper,
grasps)
if res is None:
print("Unable to plan pick")
else:
transit, approach, lift = res
qgrasp = approach.milestones[-1]
#get the grasp transform
robot.setConfig(qgrasp)
Tobj = obj.getTransform()
Tobject_grasp = se3.mul(se3.inv(gripper_link.getTransform()),
Tobj)
robot.setConfig(lift.milestones[-1])
res = place.plan_place(world, robot, obj, Tobject_grasp,
target_gripper, goal_bounds)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
trajectory_is_transfer = Trajectory()
trajectory_is_transfer.times.append(0)
trajectory_is_transfer.milestones.append([0])
traj = transit
traj = traj.concat(approach,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
trajectory_is_transfer.milestones.append([1])
traj = traj.concat(lift, relative=True, jumpPolicy='jump')
traj = traj.concat(transfer,
relative=True,
jumpPolicy='jump')
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([1])
trajectory_is_transfer.milestones.append([0])
traj = traj.concat(retract,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
solved_trajectory = traj
obj.setTransform(*Tobj0)
vis.add("traj", traj, endEffectors=[gripper_link.index])
robot.setConfig(qstart)
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
