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 callback(robot_controller=robot_controller, drawing_controller=drawing_controller, storage=[sim_world, planning_world, sim, controller_vis]): start_clock = time.time() dt = 1.0 / robot_controller.controlRate() #advance the controller robot_controller.startStep() if robot_controller.status() == 'ok': drawing_controller.advance(dt) vis.addText("Status", drawing_controller.state, (10, 20)) robot_controller.endStep() #update the visualization sim_robot.setConfig( robot_controller.configToKlampt(robot_controller.sensedPosition())) Tee = sim_robot.link(ee_link).getTransform() vis.add( "pen axis", Trajectory([0, 1], [ se3.apply(Tee, ee_local_pos), se3.apply(Tee, vectorops.madd(ee_local_pos, ee_local_axis, lifth)) ]), color=(0, 1, 0, 1)) controller_vis.update() cur_clock = time.time() duration = cur_clock - start_clock time.sleep(max(0, dt - duration))
def set_path(self): """ :return: config_list: {list} -- the list of suturing path in robot configuration space """ config_list = [] localpos = [[0.0, 0.0, 0.0], [-self.radius + self.radius * np.cos((120 * np.pi) / 180.), -self.radius * np.sin((120 * np.pi) / 180.), 0.0], [-self.radius + self.radius * np.cos((240 * np.pi) / 180.), -self.radius * np.sin((240 * np.pi) / 180.), 0.0]] center = [(self.dim_arr[0][0] * 1e-3) - ((self.suture_center[0, 0] / self.dim_arr[1][0]) * self.dim_arr[0][0] * 1e-3), (self.suture_center[1, 0] / self.dim_arr[1][1]) * self.dim_arr[0][1] * 1e-3] print("The center is: ", (self.suture_center[2, 0]/self.dim_arr[1][2]) * 1e-3) for i in range(0, self.rotation * 10): current_angle = i * 0.1 pos_x_0 = center[0] - self.radius * np.cos((current_angle * np.pi) / 180.) pos_y_0 = center[1] + self.radius * np.sin((current_angle * np.pi) / 180.) pos_x_1 = center[0] - self.radius * np.cos(((current_angle - 120) * np.pi) / 180.) pos_y_1 = center[1] + self.radius * np.sin(((current_angle - 120) * np.pi) / 180.) pos_x_2 = center[0] - self.radius * np.cos(((current_angle - 240) * np.pi) / 180.) pos_y_2 = center[1] + self.radius * np.sin(((current_angle - 240) * np.pi) / 180.) world_pos = [se3.apply(self.Toct, [pos_x_0, pos_y_0, (self.suture_center[2, 0]/self.dim_arr[1][2]) * 1e-2]), se3.apply(self.Toct, [pos_x_1, pos_y_1, (self.suture_center[2, 0]/self.dim_arr[1][2]) * 1e-2]), se3.apply(self.Toct, [pos_x_2, pos_y_2, (self.suture_center[2, 0]/self.dim_arr[1][2]) * 1e-2])] q = solve_ik(self.robot.link('tool0'), localpos, world_pos) config_list.append(q) return 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 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 apply_tendon_forces(self,i,link1,link2,rest_length): tendon_c2 = 30000.0 tendon_c1 = 10000.0 b0 = self.sim.body(self.model.robot.link(self.model.proximal_links[0]-3)) b1 = self.sim.body(self.model.robot.link(link1)) b2 = self.sim.body(self.model.robot.link(link2)) p0w = se3.apply(b1.getTransform(),self.tendon0_local) p1w = se3.apply(b1.getTransform(),self.tendon1_local) p2w = se3.apply(b2.getTransform(),self.tendon2_local) d = vectorops.distance(p1w,p2w) if d > rest_length: #apply tendon force direction = vectorops.unit(vectorops.sub(p2w,p1w)) f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length) #print d,rest_length #print "Force magnitude",self.model.robot.link(link1).getName(),":",f f = min(f,100) #tendon routing force straight = vectorops.unit(vectorops.sub(p2w,p0w)) pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w)) pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0)) tangential_axis = vectorops.cross(pulley_axis,pulley_direction) cosangle = vectorops.dot(straight,tangential_axis) #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle base_direction = so3.apply(b0.getTransform()[0],[0,0,-1]) b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04)) b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local) b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local) b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local) self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f) self.forces[i][2] = vectorops.mul(direction,f) else: self.forces[i] = [None,None,None] return
def drawGL(self,q): x = self.getSensedValue(q) xdes = self.xdes if self.baseLinkNo >= 0: Tb = self.baseLink.getTransform() if self.taskType == "position": x = se3.apply(Tb,x) xdes = se3.apply(Tb,xdes) elif self.taskType == "po": x = se3.mul(Tb,x) xdes = se3.mul(Tb,xdes) glPointSize(6) glEnable(GL_POINT_SMOOTH) glBegin(GL_POINTS) if self.taskType == "position": glColor3f(1,0,0) #red glVertex3fv(xdes) glColor3f(1,0.5,0) #orange glVertex3fv(x) elif self.taskType == "po": glColor3f(1,0,0) #red glVertex3fv(xdes[1]) glColor3f(1,0.5,0) #orange glVertex3fv(x[1]) glEnd()
def display(self): #draw points on the robot lh = Hand('l') rh = Hand('r') glDisable(GL_LIGHTING) glPointSize(5.0) glDisable(GL_DEPTH_TEST) glBegin(GL_POINTS) glColor3f(0, 1, 0) glVertex3fv( se3.apply( self.world.robot(0).link(lh.link).getTransform(), lh.localPosition1)) glVertex3fv( se3.apply( self.world.robot(0).link(rh.link).getTransform(), rh.localPosition1)) glColor3f(0, 0, 1) glVertex3fv( se3.apply( self.world.robot(0).link(lh.link).getTransform(), lh.localPosition2)) glVertex3fv( se3.apply( self.world.robot(0).link(rh.link).getTransform(), rh.localPosition2)) glColor3f(1, 0, 0) glVertex3fv(self.world.rigidObject(0).getTransform()[1]) glEnd() glEnable(GL_DEPTH_TEST)
def drawGL(self, q): x = self.getSensedValue(q) xdes = self.xdes if self.baseLinkNo >= 0: Tb = self.baseLink.getTransform() if self.taskType == "position": x = se3.apply(Tb, x) xdes = se3.apply(Tb, xdes) elif self.taskType == "po": x = se3.mul(Tb, x) xdes = se3.mul(Tb, xdes) glPointSize(6) glEnable(GL_POINT_SMOOTH) glBegin(GL_POINTS) if self.taskType == "position": glColor3f(1, 0, 0) #red glVertex3fv(xdes) glColor3f(1, 0.5, 0) #orange glVertex3fv(x) elif self.taskType == "po": glColor3f(1, 0, 0) #red glVertex3fv(xdes[1]) glColor3f(1, 0.5, 0) #orange glVertex3fv(x[1]) glEnd()
def display(self): if self.running: self.world.drawGL() w_T_o = np.array(se3.homogeneous(self.obj.getTransform())) for pose in self.poses: w_T_p_des = w_T_o.dot(pose) w_T_p__des_se3 = se3.from_homogeneous(w_T_p_des) draw_GL_frame(w_T_p__des_se3, color=(0.5,0.5,0.5)) if self.curr_pose is not None: w_T_p_des = w_T_o.dot(self.curr_pose) w_T_p__des_se3 = se3.from_homogeneous(w_T_p_des) draw_GL_frame(w_T_p__des_se3) hand_xform = get_moving_base_xform(self.robot) w_T_p = np.array(se3.homogeneous(hand_xform)).dot(self.h_T_p) w_T_p_se3 = se3.from_homogeneous(w_T_p) draw_GL_frame(w_T_p_se3) glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) glEnable(GL_POINT_SMOOTH) glColor3f(1,1,0) glLineWidth(1.0) glPointSize(5.0) forceLen = 0.01 # scale of forces if self.sim is not None and self.obj is not None and self.robot is not None: c_p, c_f = getObjectPhalanxMeanContactPoint(self.sim, self.obj, self.robot, self.links_to_check) n_c_p = countContactPoints(c_p) if countContactPoints(c_p) > 0: glBegin(GL_POINTS) for i in range(len(c_p)/3): o_c_p_i = c_p[3*i:3*i+3] if np.all([False if math.isnan(p) else True for p in o_c_p_i]): w_c_p_i = se3.apply(se3.from_homogeneous(w_T_o), o_c_p_i) glVertex3f(*w_c_p_i) glEnd() glBegin(GL_LINES) for i in range(len(c_p)/3): o_c_p_i = c_p[3 * i:3 * i + 3] o_c_f_i = c_f[6 * i:6 * i + 3] if np.all([False if math.isnan(f) else True for f in o_c_f_i]): if np.all([False if math.isnan(p) else True for p in o_c_p_i]): w_c_p_i = se3.apply(se3.from_homogeneous(w_T_o), o_c_p_i) w_c_f_i = se3.apply(se3.from_homogeneous(w_T_o), o_c_f_i) glVertex3f(*w_c_p_i) glVertex3f(*vectorops.madd(w_c_p_i, w_c_f_i, forceLen)) glEnd()
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 transform_point_cloud(pc, T, point_channels=[0, 1, 2], normal_channels=[6, 7, 8]): """Given a point cloud `pc` and a transform T, apply the transform to the point cloud (in place). Args: pc (np.ndarray): an N x M numpy array, with N points and M channels. T (klampt se3 element): a Klamp't se3 element representing the transform to apply. point_channels (list of 3 ints): The channel indices (columns) in pc corresponding to the point data. normal_channels (list of 3 ints): The channels indices(columns) in pc corresponding to the normal data. If this is None or an index is >= M, just ignore. """ N, M = pc.shape assert len(point_channels) == 3 for i in point_channels: assert i < M, "Invalid point_channel" for i in range(N): point_data = pc[i, :] point = point_data[point_channels] point_data[point_channels] = se3.apply(T, point) if normal_channels is not None and normal_channels[0] < M: for i in normal_channels: assert i < M, "Invalid normal_channel" for i in range(N): point_data = pc[i, :] point = point_data[normal_channels] point_data[normal_channels] = so3.apply(T[0], point)
def draw_part_circle_3d(radius, center, start_Z, rot_angle, start_angle, rot_x, pix_x, pix_h, pix_z): """ :param radius: {float} -- the radius of suture path :param center: {list} -- the suture center in the OCT frame :param start_Z: {float} -- the suture path Z axis value in the OCT frame :param rot_angle: {float} -- the rotation angle of the suture path :param start_angle: {float} -- the start suture angle :param rot_x: {float} -- the suture path rotation on the OCT frame X axis :param pix_x: {float} -- the length in pixels x length :param pix_h: {float} -- the length in pixels y length :param pix_z: {float} -- the length in pixels z length :return: suture_path: {list} -- the suture milestones in the world frame """ suture_path = [] rot_mat = (so3.from_rpy([rot_x, 0.0, 0.0]), [0.0] * 3) for i in range(0, rot_angle * 10): current_angle = start_angle + i * 0.1 pos_x_0 = center[0] - radius * np.cos((current_angle * np.pi) / 180.) pos_y_0 = center[1] + radius * np.sin((current_angle * np.pi) / 180.) milestone = np.divide(se3.apply(rot_mat, [pos_x_0, pos_y_0, start_Z]), [pix_x, pix_h, pix_z]) suture_path.append(milestone) return suture_path
def callback(robot_controller=robot_controller,drawing_controller=drawing_controller,trace=trace, storage=[sim_world,planning_world,sim,controller_vis]): start_clock = time.time() dt = 1.0/robot_controller.controlRate() #advance the controller robot_controller.startStep() drawing_controller.advance(dt) robot_controller.endStep() #update the visualization sim_robot.setConfig(robot_controller.configToKlampt(robot_controller.sensedPosition())) Tee=sim_robot.link(ee_link).getTransform() wp = se3.apply(Tee,ee_local_pos) if len(trace.milestones) == 0 or vectorops.distance(wp,trace.milestones[-1]) > 0.01: trace.milestones.append(wp) trace.times.append(0) if len(trace.milestones)==2: vis.add("trace",trace,color=(0,1,1,1),pointSize=0) if len(trace.milestones) > 200: trace.milestones = trace.milestones[-100:] trace.times = trace.times[-100:] if len(trace.milestones)>2: if _backend=='IPython': vis.remove("trace") vis.add("trace",trace,color=(0,1,1,1),pointSize=0) else: vis.dirty("trace") controller_vis.update() cur_clock = time.time() duration = cur_clock - start_clock time.sleep(max(0,dt-duration))
def drawGL(self,q): x = self.getSensedValue(q) xdes = self.xdes #invert the transformations from task to world coordinates if self.baseLinkNo >= 0: Tb = self.robot.link(self.baseLinkNo).getTransform() xdes = se3.apply(Tb,self.xdes) x = se3.apply(Tb,x) glPointSize(10) glEnable(GL_POINT_SMOOTH) glBegin(GL_POINTS) glColor3f(0,1,0) #green glVertex3fv(xdes) glColor3f(0,1,1) #cyan glVertex3fv(x) glEnd()
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 df_dz(self, x, y, z): n = len(y) m = len(z) // n result = np.zeros((6, len(z))) T = self.env_geom.getTransform() CoM = se3.apply(T, self.env_obj.getMass().getCom()) for i in range(n): point = y[i][1] Normal_normal = normal(self.env_geom, point) n1 = so3.canonical(Normal_normal)[3:6] n2 = so3.canonical(Normal_normal)[6:9] Normal_friction = [] for j in range(6): n_tmp = (math.cos((math.pi / 3) * j) * np.array(n1) + math.sin( (math.pi / 3) * j) * np.array(n2)).tolist() Normal_friction.append(vectorops.unit(n_tmp)) Cross_Normal = list( vectorops.cross(vectorops.sub(point, CoM), Normal_normal)) Cross_Normal_friction = [] for j in range(6): cross_Normal_v = list( vectorops.cross(vectorops.sub(point, CoM), Normal_friction[j])) Cross_Normal_friction.append(cross_Normal_v) result[0:3, i * m:(i + 1) * m - 1] = np.asarray(Normal_friction).T result[0:3, (i + 1) * m - 1:(i + 1) * m] = np.asarray(Normal_normal).reshape((3, 1)) result[3:6, i * m:(i + 1) * m - 1] = np.asarray(Cross_Normal_friction).T result[3:6, (i + 1) * m - 1:(i + 1) * m] = np.asarray(Cross_Normal).reshape((3, 1)) return result
def drawGL(self, q): x = self.getSensedValue(q) xdes = self.xdes #invert the transformations from task to world coordinates if self.baseLinkNo >= 0: Tb = self.robot.link(self.baseLinkNo).getTransform() xdes = se3.apply(Tb, self.xdes) x = se3.apply(Tb, x) glPointSize(10) glEnable(GL_POINT_SMOOTH) glBegin(GL_POINTS) glColor3f(0, 1, 0) #green glVertex3fv(xdes) glColor3f(0, 1, 1) #cyan glVertex3fv(x) glEnd()
def simulateForceSpring(self, kP=10.0): if not self.forceApplicationMode: return self.sim.updateWorld() body = self.sim.body(self.forceObject) T = self.forceObject.getTransform() wp = se3.apply(T, self.forceLocalPoint) f = vectorops.mul(vectorops.sub(self.forceAnchorPoint, wp), kP) body.applyForceAtLocalPoint(f, self.forceLocalPoint)
def callback(self, data): self.marker = data self.marker_pos[0] = self.marker.pose.position.x self.marker_pos[1] = self.marker.pose.position.y self.marker_pos[2] = self.marker.pose.position.z #=== transform to world coordinate marker = se3.apply(kinect_frame_to_pedestal, tuple(self.marker_pos)) self.marker_pos_tf = list(marker) self.marker_pos_tf[2] = self.marker_pos_tf[2] + self.base_height
def apply_tendon_forces(self, i, link1, link2, rest_length): tendon_c2 = 30000.0 tendon_c1 = 10000.0 b0 = self.sim.body( self.model.robot.link(self.model.proximal_links[0] - 3)) b1 = self.sim.body(self.model.robot.link(link1)) b2 = self.sim.body(self.model.robot.link(link2)) p0w = se3.apply(b1.getTransform(), self.tendon0_local) p1w = se3.apply(b1.getTransform(), self.tendon1_local) p2w = se3.apply(b2.getTransform(), self.tendon2_local) d = vectorops.distance(p1w, p2w) if d > rest_length: #apply tendon force direction = vectorops.unit(vectorops.sub(p2w, p1w)) f = tendon_c2 * (d - rest_length)**2 + tendon_c1 * (d - rest_length) #print d,rest_length #print "Force magnitude",self.model.robot.link(link1).getName(),":",f f = min(f, 100) #tendon routing force straight = vectorops.unit(vectorops.sub(p2w, p0w)) pulley_direction = vectorops.unit(vectorops.sub(p1w, p0w)) pulley_axis = so3.apply(b1.getTransform()[0], (0, 1, 0)) tangential_axis = vectorops.cross(pulley_axis, pulley_direction) cosangle = vectorops.dot(straight, tangential_axis) #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle base_direction = so3.apply(b0.getTransform()[0], [0, 0, -1]) b0.applyForceAtLocalPoint( vectorops.mul(base_direction, -f), vectorops.madd(p0w, base_direction, 0.04)) b1.applyForceAtLocalPoint( vectorops.mul(tangential_axis, cosangle * f), self.tendon1_local) b1.applyForceAtLocalPoint( vectorops.mul(tangential_axis, -cosangle * f), self.tendon0_local) b2.applyForceAtLocalPoint(vectorops.mul(direction, -f), self.tendon2_local) self.forces[i][1] = vectorops.mul(tangential_axis, cosangle * f) self.forces[i][2] = vectorops.mul(direction, f) else: self.forces[i] = [None, None, None] return
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): """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): """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): """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 statesCollection(self, each_blob): ''' input: blob object output: states and covariance(None) Use invh to caculate the position of the ball, the vels are assumed to be zeros ''' cur_obs = [each_blob.x, each_blob.y, each_blob.w, each_blob.h] cur_p_c = self.hInvPosSolver(cur_obs, BALL_RADIUS, self.w / 2.0, self.h / 2.0) cur_states = se3.apply(self.Tsensor, cur_p_c.T.tolist()[0]) + [0, 0, 0] cur_cov = None return cur_states, cur_cov
def display(self): GLSimulationPlugin.display(self) #draw force springs if using if self.forceApplicationMode: self.sim.updateWorld() glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) glColor3f(1,0.5,0) glLineWidth(3.0) glBegin(GL_LINES) glVertex3fv(self.forceAnchorPoint) glVertex3fv(se3.apply(self.forceObject.getTransform(),self.forceLocalPoint)) glEnd() glLineWidth(1.0) glEnable(GL_DEPTH_TEST)
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 statesInitialization(self, each_blob, pre_xyz, num_pre_points): ''' input: blob object from observation, previous collected positions, number of points for line fitting output: position by hInvPosSolver, and vels approximated by line fitting ''' cur_obs = [each_blob.x, each_blob.y, each_blob.w, each_blob.h] cur_p_c = self.hInvPosSolver(cur_obs, BALL_RADIUS, self.w / 2.0, self.h / 2.0) cur_p_w = se3.apply(self.Tsensor, cur_p_c.T.tolist()[0]) cur_p_vel = self.velEst(pre_xyz, self.dt, num_pre_points, num_pre_points) cur_states = cur_p_w + cur_p_vel cur_cov = np.vstack((np.zeros( (3, 6)), np.hstack((np.zeros( (3, 3)), np.eye(3) * (0.1 / 2.33)**2)))) return cur_states, cur_cov
def generate(self,camera_xform,camera_intrinsics,color_image,depth_image,k='auto'): """Returns a list of k AntipodalGrasps in world space according to the given color and depth images. Returns a list of k scores as well. """ #TODO: implement me for Problem3B #tip: make your life easy and call code from problem2.py directly pixel = np.unravel_index(depth_image.argmin(), depth_image.shape) depth = depth_image[pixel] fx,fy,cx,cy = camera_intrinsics['fx'],camera_intrinsics['fy'],camera_intrinsics['cx'],camera_intrinsics['cy'] y,x = pixel Z = depth X = (x - cx)/fx*Z Y = (y - cy)/fy*Z grasp_closest = AntipodalGrasp(se3.apply(camera_xform,[X,Y,Z]),[1,0,0]) score = 1.0 return [grasp_closest],[score]
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 next(self): """Returns the next Grasp from the database.""" if self.options is None: return None if self.index >= len(self.options): self.options = None return None c, n, score = self.options(self.index) self.index += 1 cworld = se3.apply(self.pc_xform, c) nworld = so3.apply(self.pc_xform[0], n) objective = IKObjective() objective.setLinks(self.gripper.link) objective.setFixedPoint(self.gripper.center, cworld) objective.setAxialRotConstraint(self.gripper.primary_axis, vectorops.mul(nworld, -1)) return Grasp(objective, score=score)
def compute_bb_tight(geom, Rt=None): if geom.type() == "Group": return geom.getBB() #Rtc=geom.getCurrentTransform() if Rt is None else se3.mul(Rt,geom.getCurrentTransform()) #for i in range(geom.numElements()): # e=geom.getElement(i) # bb=union_bb(bb,compute_bb_tight(e,Rtc)) #return bb elif geom.type() == "TriangleMesh": bb = empty_bb(3) m = geom.getTriangleMesh() for v in range(len(m.vertices) // 3): vert = [m.vertices[v * 3 + d] for d in range(3)] if Rt is not None: vert = se3.apply(Rt, vert) bb = union_bb(bb, vert) return bb else: return None
def value(self, x, y, z): result = np.zeros(6) if len(y) == 0: result[0:3] += self.mass * self.gravity_coefficient * np.asarray( self.gravity_direction) return result n = len(y) m = len(z) // n assert m == 7 T = self.env_geom.getTransform() CoM = se3.apply(T, self.env_obj.getMass().getCom()) for i in range(n): point = y[i][1] f_normal = z[m * (i + 1) - 1] f_friction = z[m * i:m * (i + 1) - 1] Normal_normal = normal(self.env_geom, point) n1 = so3.canonical(Normal_normal)[3:6] n2 = so3.canonical(Normal_normal)[6:9] Normal_friction = [] for j in range(6): n_tmp = (math.cos((math.pi / 3) * j) * np.array(n1) + math.sin( (math.pi / 3) * j) * np.array(n2)).tolist() Normal_friction.append(vectorops.unit(n_tmp)) Cross_Normal = list( vectorops.cross(vectorops.sub(point, CoM), Normal_normal)) Cross_Normal_friction = [] for j in range(6): cross_Normal_v = list( vectorops.cross(vectorops.sub(point, CoM), Normal_friction[j])) Cross_Normal_friction.append(cross_Normal_v) result[0:3] += np.asarray(Normal_normal) * f_normal + np.asarray( f_friction) @ np.asarray(Normal_friction) result[3:6] += np.asarray(Cross_Normal) * f_normal + np.asarray( f_friction) @ np.asarray(Cross_Normal_friction) result[0:3] += self.mass * self.gravity_coefficient * np.asarray( self.gravity_direction) return result
def show_workspace(grid): vis.add("world",w) res = numpy_convert.from_numpy((lower_corner,upper_corner,grid),'VolumeGrid') g_workspace = Geometry3D(res) g_surface = g_workspace.convert('TriangleMesh',0.5) if g_surface.numElements() != 0: vis.add("reachable_boundary",g_surface,color=(1,1,0,0.5)) else: print("Nothing reachable?") Tee = robot.link(ee_link).getTransform() gpen.setCurrentTransform(*Tee) box = GeometricPrimitive() box.setAABB(lower_corner,upper_corner) gbox = Geometry3D(box) #show this if you want to debug the size of the grid domain #vis.add("box",gbox,color=(1,1,1,0.2)) vis.add("pen tip",se3.apply(Tee,ee_local_pos)) vis.loop()
def drawGL(self): #draw tendons glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) glLineWidth(4.0) glColor3f(0, 1, 1) glBegin(GL_LINES) for i in range(3): b1 = self.sim.body( self.model.robot.link(self.model.proximal_links[i])) b2 = self.sim.body( self.model.robot.link(self.model.distal_links[i])) glVertex3f(*se3.apply(b1.getTransform(), self.tendon0_local)) glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local)) glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local)) glVertex3f(*se3.apply(b2.getTransform(), self.tendon2_local)) glEnd() glLineWidth(1) glColor3f(1, 0.5, 0) glBegin(GL_LINES) fscale = 0.01 for i in range(3): b1 = self.sim.body( self.model.robot.link(self.model.proximal_links[i])) b2 = self.sim.body( self.model.robot.link(self.model.distal_links[i])) if self.forces[i][0] != None: p = se3.apply(b1.getTransform(), self.tendon0_local) glVertex3f(*p) glVertex3f(*vectorops.madd(p, self.forces[i][0], fscale)) if self.forces[i][1] != None: p = se3.apply(b1.getTransform(), self.tendon1_local) glVertex3f(*p) glVertex3f(*vectorops.madd(p, self.forces[i][1], fscale)) if self.forces[i][2] != None: p = se3.apply(b2.getTransform(), self.tendon2_local) glVertex3f(*p) glVertex3f(*vectorops.madd(p, self.forces[i][2], fscale)) glEnd() glEnable(GL_DEPTH_TEST) glEnable(GL_LIGHTING)
def drawGL(self): #draw tendons glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) glLineWidth(4.0) glColor3f(0,1,1) glBegin(GL_LINES) for i in range(3): b1 = self.sim.body(self.model.robot.link(self.model.proximal_links[i])) b2 = self.sim.body(self.model.robot.link(self.model.distal_links[i])) glVertex3f(*se3.apply(b1.getTransform(),self.tendon0_local)) glVertex3f(*se3.apply(b1.getTransform(),self.tendon1_local)) glVertex3f(*se3.apply(b1.getTransform(),self.tendon1_local)) glVertex3f(*se3.apply(b2.getTransform(),self.tendon2_local)) glEnd() glLineWidth(1) glColor3f(1,0.5,0) glBegin(GL_LINES) fscale = 0.01 for i in range(3): b1 = self.sim.body(self.model.robot.link(self.model.proximal_links[i])) b2 = self.sim.body(self.model.robot.link(self.model.distal_links[i])) if self.forces[i][0] != None: p = se3.apply(b1.getTransform(),self.tendon0_local) glVertex3f(*p) glVertex3f(*vectorops.madd(p,self.forces[i][0],fscale)) if self.forces[i][1] != None: p = se3.apply(b1.getTransform(),self.tendon1_local) glVertex3f(*p) glVertex3f(*vectorops.madd(p,self.forces[i][1],fscale)) if self.forces[i][2] != None: p = se3.apply(b2.getTransform(),self.tendon2_local) glVertex3f(*p) glVertex3f(*vectorops.madd(p,self.forces[i][2],fscale)) glEnd() glEnable(GL_DEPTH_TEST) glEnable(GL_LIGHTING)