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 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