def __init__(self): self.reset() self.color = [-1, -1, -1] #TODO: fill this in with your own camera model, if you wish self.Tsensor = None cameraRot = [0, -1, 0, 0, 0, -1, 1, 0, 0] self.w, self.h = 320, 240 self.fov = 90 self.dmax = 7 if event == 'A': #at goal post, pointing a bit up and to the left self.Tsensor = (so3.mul( so3.rotation([0, 0, 1], 0.20), so3.mul(so3.rotation([0, -1, 0], 0.25), cameraRot)), [-2.55, -1.1, 0.25]) elif event == 'B': #on ground near robot, pointing up and slightly to the left self.Tsensor = (so3.mul( so3.rotation([1, 0, 0], -0.10), so3.mul(so3.rotation([0, -1, 0], math.radians(90)), cameraRot)), [-1.5, -0.5, 0.25]) self.w = 640 self.h = 480 self.dmax = 10 else: #on ground near robot, pointing to the right self.Tsensor = (cameraRot, [-1.5, -0.5, 0.25]) self.dt = 0.02 return
def opt_error_fun(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 """ Roct = so3.from_rpy(est_input[0:3]) toct = est_input[3:6] Rn = so3.from_rpy(est_input[6:9]) tn = est_input[9:12] Tlink_set = args[0] Tneedle2oct_icp = args[1] fun_list = np.array([]) for i in range(len(Tlink_set)): fun_list = np.append( fun_list, np.multiply( so3.error(so3.inv(Tneedle2oct_icp[i][0]), so3.mul(so3.mul(so3.inv(Roct), Tlink_set[i][0]), Rn)), 1)) fun_list = np.append( fun_list, np.multiply( vectorops.sub( vectorops.sub([0., 0., 0.], so3.apply(so3.inv(Tneedle2oct_icp[i][0]), Tneedle2oct_icp[i][1])), so3.apply( so3.inv(Roct), vectorops.add(vectorops.sub(Tlink_set[i][1], toct), so3.apply(Tlink_set[i][0], tn)))), 1000)) return fun_list
def __init__(self): cameraRot = [0, -1, 0, 0, 0, -1, 1, 0, 0] #at goal post, pointing a bit up and to the left self.Tsensor = (so3.mul( so3.rotation([0, 0, 1], 0.20), so3.mul(so3.rotation([0, -1, 0], 0.25), cameraRot)), [-2.55, -1.1, 0.25]) self.T_r = np.matrix(self.Tsensor[0]).reshape((3, 3)).T self.T_t = np.matrix(self.Tsensor[1]).reshape((3, 1)) self.fov = 90 self.w, self.h = 320, 240 self.dmax = 5 self.dt = 0.02 self.f = (0.5 * self.w) / np.tan(self.fov / 2.0 / 180 * np.pi) # Kalman Filter parameters self.A = np.vstack((np.hstack((np.zeros( (3, 3)), np.eye(3))), np.zeros((3, 6)))) self.dyn_noise = np.vstack((np.hstack( (np.eye(3) * (0.5 * self.dt**2 * 0.008)**2, np.zeros( (3, 3)))), np.hstack((np.zeros( (3, 3)), np.eye(3) * 0.008)))) # covariance self.F = np.eye(self.A.shape[0]) + self.A * self.dt self.g = np.vstack((np.zeros((5, 1)), np.matrix([-GRAVITY]))) * self.dt self.H_jaccobian = None # use calJaccobian to calculate this when there is some estimated state to use self.obs_noise = np.diag([0.25, 0.25, 0.5, 0.5]) # prior self.previous_estimates = dict() self.previous_cov = dict() return
def sample_object_pose_table(obj, stable_fs, bmin, bmax): """Samples a transform of the object so that it lies on in the given bounding box bmin,bmax. Args: obj (RigidObjectModel) stable_fs (list of lists): giving the stable faces of the object, as in MP2. bmin,bmax (3-vectors): the bounding box of the area in which the objects should lie. """ table_height = bmin[2] + 0.001 face = random.choice(stable_fs) normal = np.cross(face[1] - face[0], face[2] - face[0]) normal = normal / np.linalg.norm(normal) centroid = np.sum(face, axis=0) / len(face) x = random.uniform(bmin[0], bmax[0]) y = random.uniform(bmin[1], bmax[1]) z = table_height + np.dot(centroid, normal) Rnormal = so3.canonical((-normal).tolist()) Rx = so3.rotation((1, 0, 0), random.uniform(0, math.pi * 2)) Rxz = so3.rotation((0, 1, 0), -math.pi * 0.5) R = so3.mul(Rxz, so3.mul(Rx, so3.inv(Rnormal))) #R*com + t = [x,y,_] t = vectorops.sub([x, y, z], so3.apply(R, obj.getMass().getCom())) t[2] = z obj.setTransform(R, t)
def euler_angle_to_rotation(ea,convention='zyx'): """Converts an euler angle representation to a rotation matrix. Can use arbitrary axes specified by the convention arguments (default is 'zyx', or roll-pitch-yaw euler angles). Any 3-letter combination of 'x', 'y', and 'z' are accepted. """ axis_names_to_vectors = dict([('x',(1,0,0)),('y',(0,1,0)),('z',(0,0,1))]) axis0,axis1,axis2=convention R0 = so3.rotation(axis_names_to_vectors[axis0],ea[0]) R1 = so3.rotation(axis_names_to_vectors[axis1],ea[1]) R2 = so3.rotation(axis_names_to_vectors[axis2],ea[2]) return so3.mul(R0,so3.mul(R1,R2))
def __init__(self): self.reset() #TODO: fill this in with your own camera model, if you wish self.Tsensor = None cameraRot = [0, -1, 0, 0, 0, -1, 1, 0, 0] self.w, self.h = 320, 240 self.fov = 90 self.dmax = 5 self.dt = 0.02 if event == 'A': #at goal post, pointing a bit up and to the left self.Tsensor = (so3.mul( so3.rotation([0, 0, 1], 0.20), so3.mul(so3.rotation([0, -1, 0], 0.25), cameraRot)), [-2.55, -1.1, 0.25]) # Kalman Filter parameters self.A = np.vstack((np.hstack((np.zeros( (3, 3)), np.eye(3))), np.zeros((3, 6)))) self.dyn_noise = np.vstack((np.hstack( (np.eye(3) * (0.5 * self.dt**2 * 0.008)**2, np.zeros( (3, 3)))), np.hstack((np.zeros( (3, 3)), np.eye(3) * 0.008)))) # covariance self.F = np.eye(self.A.shape[0]) + self.A * self.dt self.g = np.vstack((np.zeros( (5, 1)), np.matrix([-GRAVITY]))) * self.dt self.H_jaccobian = None # use calJaccobian to calculate this when there is some estimated state to use self.obs_noise = np.diag([0.25, 0.25, 0.5, 0.5]) elif event == 'B': #on ground near robot, pointing up and slightly to the left self.Tsensor = (so3.mul( so3.rotation([1, 0, 0], -0.10), so3.mul(so3.rotation([0, -1, 0], math.radians(90)), cameraRot)), [-1.5, -0.5, 0.25]) self.w = 640 self.h = 480 self.dmax = 10 else: #on ground near robot, pointing to the right self.Tsensor = (cameraRot, [-1.5, -0.5, 0.25]) self.T_r = np.matrix(self.Tsensor[0]).reshape((3, 3)).T self.T_t = np.matrix(self.Tsensor[1]).reshape((3, 1)) self.f = (0.5 * self.w) / np.tan(self.fov / 2.0 / 180 * np.pi) # prior self.previous_estimates = dict() self.previous_cov = dict() return
def get_desired_end_effector_rotation(self, leg_number, yaw_rads): if leg_number == 1 or leg_number == 3: r = [0, 0, -1, 0, -1, 0, -1, 0, 0] else: r = [0, 0, -1, 0, 1, 0, 1, 0, 0] yaw_rotation_aa = ([0, 0, 1], yaw_rads) yaw_rotation_R = so3.from_axis_angle(yaw_rotation_aa) return so3.mul(yaw_rotation_R, r)
def xy_randomize(obj,bmin,bmax): R,t = obj.getTransform() obmin,obmax = obj.geometry().getBB() w = 0.5*(obmax[0]-obmin[0]) h = 0.5*(obmax[1]-obmin[1]) correction = max(w,h) R = so3.mul(so3.rotation([0,0,1],random.uniform(0,math.pi*2)),R) t[0] = random.uniform(bmin[0]+correction,bmax[0]-correction) t[1] = random.uniform(bmin[1]+correction,bmax[1]-correction) obj.setTransform(R,t)
def getDesiredCartesianPose(self,limb,device): """Returns a pair (R,t) of the desired EE pose if the limb should have a cartesian pose message, or None if it should not. - limb: either 'left' or 'right' - device: an index of the haptic device Implementation-wise, this reads from self.startTransforms and the deviceState to determine the correct desired end effector transform. The delta from devices[device]['deviceCurrentTransform'] to devices[device]['deviceInitialTransform'] is scaled, then offset by self.startTransforms[device]. (self.startTransforms is the end effector transform when deviceInitialTransform is set) """ if limb=='left': T = self.serviceThread.eeGetter_left.get() else: T = self.serviceThread.eeGetter_right.get() if T is None: T = se3.identity() R,t=T deviceState = self.haptic_client.deviceState if deviceState == None: return T dstate = deviceState[device] if dstate.widgetInitialTransform[0] == None or self.startTransforms[device] == None: return T Tcur = dstate.widgetCurrentTransform T0 = dstate.widgetInitialTransform[0] if T0 == None: T0 = Tcur #print "Button is down and mode is",dstate['mode'] #print dstate assert T0 != None,"T0 is null" assert Tcur != None,"Tcur is null" relRot = so3.mul(Tcur[0],so3.inv(T0[0])) relTrans = vectorops.sub(Tcur[1],T0[1]) #print "Rotation moment",so3.moment(relRot) desRot = so3.mul(relRot,self.startTransforms[device][0]) desPos = vectorops.add(vectorops.mul(relTrans,hapticArmTranslationScaling),self.startTransforms[device][1]) #TEST: just use start rotation #desRot = self.startTransforms[i][0] return (desRot,desPos)
def xy_randomize(obj,bmin,bmax): """Randomizes the xy position and z orientation of an object inside of a bounding box bmin->bmax. Assumes the bounding box is large enough to hold the object in any orientation. """ R,t = obj.getTransform() R = so3.mul(so3.rotation([0,0,1],random.uniform(0,math.pi*2)),R) obj.setTransform(R,t) obmin,obmax = obj.geometry().getBB() t[0] = random.uniform(bmin[0]-obmin[0],bmax[0]-obmax[0]) t[1] = random.uniform(bmin[1]-obmin[1],bmax[1]-obmax[1]) obj.setTransform(R,t)
def xy_randomize(obj,bmin,bmax): """Randomizes the xy position and z orientation of an object inside of a bounding box bmin->bmax. Assumes the bounding box is large enough to hold the object in any orientation. """ R,t = obj.getTransform() R = so3.mul(so3.rotation([0,0,1],random.uniform(0,math.pi*2)),R) obj.setTransform(R,t) obmin,obmax = obj.geometry().getBB() t[0] = random.uniform(bmin[0]-obmin[0],bmax[0]-obmax[0]) t[1] = random.uniform(bmin[1]-obmin[1],bmax[1]-obmax[1]) obj.setTransform(R,t)
def so3_grid(N): """Returns a nx.Graph describing a grid on SO3 with resolution N. The resulting graph has ~4N^3 nodes The node attribute 'params' gives the so3 element. """ assert N >= 1 G = nx.Graph() R0 = so3.from_quaternion(vectorops.unit((1, 1, 1, 1))) namemap = dict() for ax in range(4): for i in xrange(N + 1): u = 2 * float(i) / N - 1.0 u = math.tan(u * math.pi * 0.25) for j in xrange(N + 1): v = 2 * float(j) / N - 1.0 v = math.tan(v * math.pi * 0.25) for k in xrange(N + 1): w = 2 * float(k) / N - 1.0 w = math.tan(w * math.pi * 0.25) idx = [i, j, k] idx = idx[:ax] + [N] + idx[ax:] idx = tuple(idx) if idx in namemap: continue else: namemap[idx] = idx flipidx = tuple([N - x for x in idx]) namemap[flipidx] = idx q = [u, v, w] q = q[:ax] + [1.0] + q[ax:] #print idx,"->",q q = vectorops.unit(q) R = so3.mul(so3.inv(R0), so3.from_quaternion(q)) G.add_node(idx, params=R) for ax in range(4): for i in xrange(N + 1): for j in xrange(N + 1): for k in xrange(N + 1): baseidx = [i, j, k] idx = baseidx[:ax] + [N] + baseidx[ax:] idx = tuple(idx) for n in range(3): nidx = baseidx[:] nidx[n] += 1 if nidx[n] > N: continue nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) G.add_edge(namemap[idx], namemap[nidx]) return G
def xyz_rpy(xyz=None, rpy=None): xyz = xyz or [0, 0, 0] rpy = rpy or [0, 0, 0] Rall = None for (i, angle) in enumerate(rpy): axis = [0, 0, 0] axis[i] = 1 R = so3.rotation(axis, angle) if Rall: Rall = so3.mul(Rall, R) else: Rall = R return klampt2numpy((Rall, xyz))
def set_pos_on_palm(self, link_idx, pos, tilted_angle): ''' change the position of link mounted on the palm to x, y :param pos: tuple value (r, theta) on palm coordinate :param link_idx: indicate link :return: ''' if np.abs(tilted_angle) > 30: return print('pass') x = pos[0] * np.cos(pos[1] / 180 * np.pi) y = pos[0] * np.sin(pos[1] / 180 * np.pi) p_T = [x, y, 0] angle = np.arctan2(x, y) R_1 = so3.rotation([1, 0, 0], angle + math.radians(tilted_angle)) R_2 = (0, 0, -1, 0, -1, 0, -1, 0, 0) p_R = so3.mul(R_2, R_1) self.robot.link(link_idx).setParentTransform(p_R, p_T)
def control_loop(self): if self.sim.getTime()-self.last_end_time>0.5: if self.trajectory: if self.t<len(self.trajectory): robot=self.sim.world.robot(0) old_T=robot.link(ee_link).getTransform() old_R,old_t=old_T self.robotController.setLinear(self.trajectory[self.t][1]['robot'],self.trajectory[self.t][0]-(self.sim.getTime()-self.old_time)) robot.setConfig(self.trajectory[self.t][1]['robot']) obj=self.sim.world.rigidObject(self.target) #get a SimBody object body = self.sim.body(obj) # print self.trajectory[self.t][1]['vacuum'] if self.trajectory[self.t][1]['simulation']==1: T=body.getTransform() R,t=T body.enableDynamics(False) body.setVelocity((0,0,0),(0,0,0)) if self.flag==0: self.flag=1 self.T=robot.link(ee_link).getLocalPosition(t) # print 'here' new_T=robot.link(ee_link).getTransform() new_R,new_t=new_T change_R=so3.mul(new_R,so3.inv(old_R)) R=so3.mul(change_R,R) body.setTransform(R,robot.link(ee_link).getWorldPosition(self.T)) else: body.enableDynamics(True) self.flag=0 # print 'current config', robot.getConfig() # print 'goal config',self.trajectory[self.t][1]['robot'] # if vectorops.distance(robot.getConfig(),self.trajectory[self.t][1]['robot'])<0.01: # print 'get there' if self.sim.getTime()-self.old_time>self.trajectory[self.t][0]: self.t+=1 self.old_time=self.sim.getTime() if self.t==len(self.trajectory): self.last_end_time=self.sim.getTime() else: self.score+=1 self.in_box.append(self.target) if self.task=='pick': self.target+=1 if self.target>=self.total_object: self.task='stow' if self.task=='stow': self.target-=1 print "one pick task is done!" self.t=0 self.trajectory=[] print 'score:',self.score time.sleep(1) if self.score==2*self.total_object: print "all items are picked up!" print self.sim.world.robot(0).getConfig() exit() else: if self.score<self.total_object: target_item={} target_box={} if max(self.sim.world.rigidObject(self.target).getVelocity()[0])>0.01: print 'turning!' return target_item["position"]=self.sim.world.rigidObject(self.target).getTransform()[1] self.place_position.append(self.sim.world.rigidObject(self.target).getTransform()[1]) target_item["vacuum_offset"]=[0,0,0.1] target_item["bbox"]=self.sim.world.rigidObject(self.target).geometry().getBB() target_item['drop offset']=[0,0,0.2] target_box["box_limit"]=[[-0.5,0.2,0.1],[-0.35,0.45,0.4]] # target_box["drop position"]=[-0.4,0.3,0.2] target_box["drop position"]=[] target_box['position']=[0.2,0.8,0.15] old_config=self.sim.world.robot(0).getConfig() self.trajectory=planner.pick_up(self.sim.world,target_item,target_box,self.target) if self.trajectory==False: self.score+=1 self.target+=1 if self.score==self.total_object: self.task='stow' self.target-=1 else: self.old_time=self.sim.getTime() self.time_count=0 print "validing trajectory..." max_change_joint=0 max_joint_speed=0 for i in range(len(self.trajectory)): new_config=self.trajectory[i][1]['robot'] d_config=max(max(vectorops.sub(new_config,old_config)),-min(vectorops.sub(new_config,old_config))) speed_config=d_config/self.trajectory[i][0] if d_config>max_change_joint: max_change_joint=d_config if speed_config>max_joint_speed: max_joint_speed=speed_config old_config=new_config print 'max joint change is :', max_change_joint/3.14159*180 print 'max joint speed is:', max_joint_speed/3.14159*180 else: target_item={} target_box={} # print self.sim.world.rigidObject(self.target).getVelocity()[0] if max(self.sim.world.rigidObject(self.target).getVelocity()[0])>0.01: print 'turning!' return target_item["position"]=self.sim.world.rigidObject(self.target).getTransform()[1] target_item["vacuum_offset"]=[0,0,0.1] target_item['drop offset']=[0,0,0.15] target_item["bbox"]=self.sim.world.rigidObject(self.target).geometry().getBB() # target_box["drop position"]=self.place_position[self.target] # target_box['position']=self.place_position[self.target] target_box["box_limit"]=[[-0.2,0.3,0.1],[0.6,0.5,0.4]] # print self.place_position[self.target] target_box["drop position"]=[] target_box['position']=[] old_config=self.sim.world.robot(0).getConfig() self.trajectory=planner.stow(self.sim.world,target_item,target_box,self.target) if self.trajectory==False: self.score+=1 self.target-=1 if self.score==2*self.total_object: print 'done!' exit() else: self.old_time=self.sim.getTime() self.time_count=0 print "validing trajectory..." max_change_joint=0 max_joint_speed=0 for i in range(len(self.trajectory)): new_config=self.trajectory[i][1]['robot'] d_config=max(max(vectorops.sub(new_config,old_config)),-min(vectorops.sub(new_config,old_config))) speed_config=d_config/self.trajectory[i][0] if d_config>max_change_joint: max_change_joint=d_config if speed_config>max_joint_speed: max_joint_speed=speed_config old_config=new_config print 'max joint change is :', max_change_joint/3.14159*180 print 'max joint speed is:', max_joint_speed/3.14159*180
def onMessage(self, msg): #print "Getting haptic message" #print msg self.numMessages += 1 if msg['type'] != 'MultiHapticState': print "Strange message type", msg['type'] return if len(self.deviceState) == 0: print "Adding", len(msg['devices']) - len( self.deviceState), "haptic devices" while len(self.deviceState) < len(msg['devices']): self.deviceState.append(HapticDeviceState()) if len(msg['devices']) != len(self.deviceState): print "Incorrect number of devices in message:", len( msg['devices']) return #read button presses for dstate in self.deviceState: dstate.buttonPressed[0] = dstate.buttonPressed[1] = False dstate.buttonReleased[0] = dstate.buttonReleased[1] = False if 'events' in msg: for e in msg['events']: #print e['type'] if e['type'] == 'b1_down': dstate = self.deviceState[e['device']] dstate.buttonPressed[0] = True elif e['type'] == 'b2_down': dstate = self.deviceState[e['device']] dstate.buttonPressed[1] = True elif e['type'] == 'b1_up': dstate = self.deviceState[e['device']] dstate.buttonReleased[0] = True elif e['type'] == 'b2_up': dstate = self.deviceState[e['device']] dstate.buttonReleased[1] = True for i in range(len(self.deviceState)): dmsg = msg['devices'][i] dstate = self.deviceState[i] if dstate.time == dmsg['time']: #no update... continue dstate.newupdate = True # ===== read from msg ===== dstate.position = dmsg['position'] dstate.rotationMoment = dmsg['rotationMoment'] dstate.velocity = dmsg['velocity'] dstate.angularVelocity = dmsg['angularVelocity'] dstate.jointAngles = dmsg['jointAngles'] #dstate.gimbalAngle = dmsg['gimbalAngle'] oldTime = dstate.time dstate.time = dmsg['time'] #print "position",dmsg['position'] #print "rotation moment",dmsg['rotationMoment'] #print "angular velocity",dmsg['angularVelocity'] dstate.widgetCurrentTransform = deviceToWidgetTransform( so3.from_moment(dmsg['rotationMoment']), dmsg['position']) if dmsg['button1'] == 1: oldButtonDown = dstate.buttonDown[0] dstate.buttonDown[0] = True if not oldButtonDown: dstate.widgetInitialTransform[ 0] = dstate.widgetPreviousTransform continue else: dstate.buttonDown[0] = False if dmsg['button2'] == 1: oldButtonDown = dstate.buttonDown[1] dstate.buttonDown[1] = True if not oldButtonDown: dstate.widgetInitialTransform[ 1] = dstate.widgetPreviousTransform continue else: dstate.buttonDown[1] = False newTime = dstate.time if newTime != oldTime and dstate.widgetPreviousTransform is not None: # print "previous position = ", dstate.widgetPreviousTransform[1] # print "current position = ", dstate.widgetCurrentTransform[1] timeInterval = newTime - oldTime #print "========================" #print "time = ", timeInterval delta_Pos = vectorops.sub(dstate.widgetCurrentTransform[1], dstate.widgetPreviousTransform[1]) vel = vectorops.div(delta_Pos, timeInterval) delta_Moment = so3.moment( so3.mul(dstate.widgetCurrentTransform[0], so3.inv(dstate.widgetPreviousTransform[0]))) angvel = vectorops.div(delta_Moment, timeInterval) # print "vel = [%2.4f %2.4f %2.4f]" % (vel[0], vel[1], vel[2]) # print "angvel = [%2.4f %2.4f %2.4f]" % (angvel[0], angvel[1], angvel[2]) dstate.linearVel = list(vel) dstate.angularVel = list(angvel) #end of loop, store previous transform dstate.widgetPreviousTransform = dstate.widgetCurrentTransform
world = klampt.WorldModel() world.readFile("../../data/simulation_test_worlds/sensortest.xml") #world.readFile("../../data/tx90scenario0.xml") robot = world.robot(0) vis.add("world", world) sim = klampt.Simulator(world) sensor = sim.controller(0).sensor("rgbd_camera") print("sensor.getSetting('link'):", sensor.getSetting("link")) print("sensor.getSetting('Tsensor'):", sensor.getSetting("Tsensor")) input("Press enter to continue...") #T = (so3.sample(),[0,0,1.0]) T = (so3.mul(so3.rotation([1, 0, 0], math.radians(-10)), [1, 0, 0, 0, 0, -1, 0, 1, 0]), [0, -2.0, 0.5]) sensing.set_sensor_xform(sensor, T, link=-1) vis.add("sensor", sensor) read_local = True #Note: GLEW sensor simulation only runs if it occurs in the visualization thread (e.g., the idle loop) class SensorTestWorld(GLPluginInterface): def __init__(self): GLPluginInterface.__init__(self) robot.randomizeConfig() #sensor.kinematicSimulate(world,0.01) sim.controller(0).setPIDCommand(robot.getConfig(), [0.0] * 7)
def deviceToViewTransform(R, t): Ruser = so3.mul(so3.inv(handleToUser[0]), R) R = so3.mul(userToWorld[0], so3.mul(Ruser, worldToUser[0])) return (R, so3.apply([0, 1, 0, 0, 0, 1, 1, 0, 0], t))
def drive(self, qcur, angVel, vel, dt): """Drives the robot by an incremental time step to reach the desired Cartesian (angular/linear) velocities of the links previously specified in start(). Args: qcur (list of floats): The robot's current configuration. angVel (3-vector or list of 3-vectors): the angular velocity of each driven link. Set angVel to None to turn off orientation control of every constraint. angVel[i] to None to turn off orientation control of constraint i. vel (3-vector or list of 3-vectors): the linear velocity of each driven link. Set vel to None to turn off position control of every constraint. Set vel[i] to None to turn off position control of constraint i. dt (float): the time step. Returns: tuple: A pair ``(progress,qnext)``. ``progress`` gives a value in the range [0,1] indicating indicating how far along the nominal drive amount the solver was able to achieve. If the result is < 0, this indicates that the solver failed to make further progress. ``qnext`` is the resulting configuration that should be sent to the robot's controller. For longer moves, you should pass qnext back to this function as qcur. """ if angVel is None: #turn off orientation control if vel is None: #nothing specified return (1.0, qcur) angVel = [None] * len(self.links) else: assert len(angVel) > 0 if not hasattr(angVel[0], '__iter__'): angVel = [angVel] if vel is None: #turn off position control vel = [None] * len(self.links) else: assert len(vel) > 0 if not hasattr(vel[0], '__iter__'): vel = [vel] if len(qcur) != self.robot.numLinks(): raise ValueError( "Invalid size of current configuration, %d != %d" % (len(qcur), self.robot.numLinks())) if len(angVel) != len(self.links): raise ValueError( "Invalid # of angular velocities specified, %d != %d" % (len(angVel), len(self.links))) if len(vel) != len(self.links): raise ValueError( "Invalid # of translational velocities specified, %d != %d" % (len(vel), len(self.links))) anyNonzero = False zeroVec = [0, 0, 0] for (w, v) in zip(angVel, vel): if w is not None and list(w) != zeroVec: anyNonzero = True break if v is not None and list(v) != zeroVec: anyNonzero = True break if not anyNonzero: return (1.0, qcur) qout = [v for v in qcur] #update drive transforms self.robot.setConfig(qcur) robotLinks = [self.robot.link(l) for l in self.links] #limit the joint movement by joint limits and velocity tempqmin, tempqmax = self.robot.getJointLimits() if self.qmin is not None: tempqmin = self.qmin if self.qmax is not None: tempqmax = self.qmax vmax = self.robot.getVelocityLimits() vmin = vectorops.mul(vmax, -1) if self.vmin is not None: vmin = self.vmin if self.vmax is not None: vmax = self.vmax for i in range(self.robot.numLinks()): tempqmin[i] = max(tempqmin[i], qcur[i] + dt * vmin[i]) tempqmax[i] = min(tempqmax[i], qcur[i] + dt * vmax[i]) #Setup the IK solver parameters self.solver.setJointLimits(tempqmin, tempqmax) tempGoals = [ik.IKObjective(g) for g in self.ikGoals] for i in range(len(self.links)): if math.isfinite(self.positionTolerance) and math.isfinite( self.rotationTolerance): tempGoals[i].rotationScale = self.positionTolerance / ( self.positionTolerance + self.rotationTolerance) tempGoals[i].positionScale = self.rotationTolerance / ( self.positionTolerance + self.rotationTolerance) elif not math.isfinite( self.positionTolerance) and not math.isfinite( self.rotationTolerance): pass else: tempGoals[i].rotationScale = min( self.positionTolerance, self.rotationTolerance) / self.rotationTolerance tempGoals[i].positionScale = min( self.positionTolerance, self.rotationTolerance) / self.positionTolerance tolerance = min( 1e-6, min(self.positionTolerance, self.rotationTolerance) / (math.sqrt(3.0) * len(self.links))) self.solver.setTolerance(tolerance) #want to solve max_t s.t. there exists q satisfying T_i(q) = TPath_i(t) #where TPath_i(t) = Advance(Tdrive_i,Screw_i*t) if self.driveSpeedAdjustment == 0: self.driveSpeedAdjustment = 0.1 anyFailures = False while self.driveSpeedAdjustment > 0: #advance the desired cartesian goals #set up IK parameters: active dofs, IKGoal amount = dt * self.driveSpeedAdjustment desiredTransforms = [[None, None] for i in range(len(self.links))] for i in range(len(self.links)): if vel[i] is not None: desiredTransforms[i][1] = vectorops.madd( self.driveTransforms[i][1], vel[i], amount) tempGoals[i].setFixedPosConstraint( self.endEffectorOffsets[i], desiredTransforms[i][1]) else: tempGoals[i].setFreePosition() if angVel[i] is not None: increment = so3.from_moment( vectorops.mul(angVel[i], amount)) desiredTransforms[i][0] = so3.mul( increment, self.driveTransforms[i][0]) tempGoals[i].setFixedRotConstraint(desiredTransforms[i][0]) else: tempGoals[i].setFreeRotConstraint() self.solver.set(i, tempGoals[i]) err0 = self.solver.getResidual() quality0 = vectorops.norm(err0) res = self.solver.solve() q = self.robot.getConfig() activeDofs = self.solver.getActiveDofs() for k in activeDofs: if q[k] < tempqmin[k] or q[k] > tempqmax[k]: #the IK solver normalizer doesn't care about absolute #values for joints that wrap around 2pi if tempqmin[k] <= q[k] + 2 * math.pi and q[ k] + 2 * math.pi <= tempqmax[k]: q[k] += 2 * math.pi elif tempqmin[k] <= q[k] - 2 * math.pi and q[ k] - 2 * math.pi <= tempqmax[k]: q[k] -= 2 * math.pi else: print( "CartesianDriveSolver: Warning, result from IK solve is out of bounds: index", k, ",", tempqmin[k], "<=", q[k], "<=", tempqmax[k]) q[k] = max(min(q[k], tempqmax[k]), tempqmin[k]) self.robot.setConfig(q) #now evaluate quality of the solve errAfter = self.solver.getResidual() qualityAfter = vectorops.norm(errAfter) if qualityAfter > quality0: #print("CartesianDriveSolver: Solve failed: original configuration was better:",quality0,"vs",qualityAfter) #print(" solver result was",res,"increment",amount) res = False elif qualityAfter < quality0 - 1e-8: res = True if res: #success! for k in activeDofs: qout[k] = q[k] assert tempqmin[k] <= q[k] and q[k] <= tempqmax[k] #now advance the driven transforms #figure out how much to drive along screw numerator = 0 # this will get sum of distance * screws denominator = 0 # this will get sum of |screw|^2 for all screws #result will be numerator / denominator achievedTransforms = [ (l.getTransform()[0], l.getWorldPosition(ofs)) for l, ofs in zip(robotLinks, self.endEffectorOffsets) ] #TODO: get transforms relative to baseLink for i in range(len(self.links)): if vel[i] is not None: trel = vectorops.sub(achievedTransforms[i][1], self.driveTransforms[i][1]) vellen = vectorops.norm(vel[i]) axis = vectorops.div(vel[i], max(vellen, 1e-8)) ut = vellen tdistance = vectorops.dot(trel, axis) tdistance = min(max(tdistance, 0.0), dt * vellen) numerator += ut * tdistance denominator += ut**2 if angVel[i] is not None: Rrel = so3.mul(achievedTransforms[i][0], so3.inv(self.driveTransforms[i][0])) vellen = vectorops.norm(angVel[i]) angVelRel = so3.apply( so3.inv(self.driveTransforms[i][0]), angVel[i]) rotaxis = vectorops.div(angVelRel, max(vellen, 1e-8)) Rdistance = axis_rotation_magnitude(Rrel, rotaxis) Rdistance = min(max(Rdistance, 0.0), dt * vellen) uR = vellen numerator += uR * Rdistance denominator += uR**2 distance = numerator / max(denominator, 1e-8) #computed error-minimizing distance along screw motion for i in range(len(self.links)): if vel[i] is not None: self.driveTransforms[i][1] = vectorops.madd( self.driveTransforms[i][1], vel[i], distance) else: self.driveTransforms[i][1] = achievedTransforms[i][1] if angVel[i] is not None: Rincrement = so3.from_moment( vectorops.mul(angVel[i], distance)) self.driveTransforms[i][0] = normalize_rotation( so3.mul(Rincrement, self.driveTransforms[i][0])) else: self.driveTransforms[i][0] = achievedTransforms[i][0] if math.ceil(distance / dt * 10) < math.floor( self.driveSpeedAdjustment * 10): self.driveSpeedAdjustment -= 0.1 self.driveSpeedAdjustment = max(self.driveSpeedAdjustment, 0.0) elif not anyFailures: #increase drive velocity if self.driveSpeedAdjustment < 1.0: self.driveSpeedAdjustment += 0.1 self.driveSpeedAdjustment = min( 1.0, self.driveSpeedAdjustment) return (distance / dt, qout) else: #IK failed: try again with a slower speed adjustment anyFailures = True self.driveSpeedAdjustment -= 0.1 if self.driveSpeedAdjustment <= 1e-8: self.driveSpeedAdjustment = 0.0 break #no progress return (-1, qcur)
def viewToDeviceTransform(R, t): Ruser = so3.mul(worldToUser[0], so3.mul(R, userToWorld[0])) R = so3.mul(handleToUser[0], Ruser) return (R, so3.apply(so3.inv([0, 1, 0, 0, 0, 1, 1, 0, 0]), t))
import math #two finger configurations c_hand = 0.28 o_hand = 0.6 power_hand = 0 precision_hand = 0.75 power_close_hand = [c_hand, c_hand, c_hand, power_hand] power_open_hand = [o_hand - 0.02, o_hand - 0.02, o_hand - 0.02, power_hand] precision_close_hand = [c_hand, c_hand, c_hand, precision_hand] precision_open_hand = [o_hand, o_hand, o_hand, precision_hand] #hand orientation move_speed = 0.5 face_down = [0, -1, 0, 1, 0, 0, 0, 0, 1] face_down_rot = [-1, 0, 0, 0, -1, 0, 0, 0, 1] # face_toward_shelf=[1,0,0,0,0,1,0,-1,0] face_toward_shelf = so3.mul( so3.from_axis_angle(([1, 0, 0], math.pi / 180 * 70)), face_down) idle_pos = (face_down, [0, 0.5, 0.77]) start_pos_face_down = (face_down, [0, 0.4, 0.77]) start_pos_face_down_rot = (face_down_rot, [0, 0.4, 0.77]) start_pos_face_toward_shelf = (face_toward_shelf, [0, 0.4, 0.77]) drop_pos_face_down = (face_down, [0, -0.1, 0.77]) drop_pos_face_down_rot = (face_down_rot, [0, -0.1, 0.77]) drop_pos_face_toward_shelf = (face_toward_shelf, [0, -0.2, 0.77]) sweep_pos = so3.mul(so3.from_axis_angle(([1, 0, 0], math.pi / 180 * 70)), face_down_rot) start_pos_sweep = (sweep_pos, [0, 0.55, 0.77]) drop_pos_sweep = (sweep_pos, [0, -0.1, 0.77]) # sweep_pos_1=(face_down,[-0.03,0.85,0.7]) # sweep_pos_2=(face_down,[-0.03,0.85,0.59])
def rotationDeltaFromButtonPress(self, button=0): """Returns the difference between the current rotation and the button press rotation, relative to the frame of the button press rotation""" return so3.mul(self.widgetCurrentTransform[0], so3.inv(self.widgetInitialTransform[button][0]))
def control_loop(self): #Calculate the desired velocity for each robot by adding up all #commands rvels = [[0] * self.world.robot(r).numLinks() for r in range(self.world.numRobots())] robot = self.world.robot(0) ee_link = 6 ee_local_p1 = [-0.015, -0.02, 0.27] q_curr = robot.getConfig() q_curr[3] = -q_curr[3] q_curr[5] = -q_curr[5] q_output = vectorops.mul(q_curr[1:], angle_to_degree) milestone = (0.1, { 'robot': q_output, 'gripper': [0, 0, 0], 'vaccum': [0] }) self.output.append(milestone) if self.start_flag == 0: curr_position = robot.link(ee_link).getWorldPosition(ee_local) curr_orientation, p = robot.link(ee_link).getTransform() self.start_T = [curr_orientation, curr_position] self.start_flag = 1 local_p = [] if self.state == 'idle': t = 0.5 if self.score == self.numberOfObjects: print 'finished!' f = open('test.json', 'w') json.dump(self.output, f) f.close() exit() else: if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t self.end_T = [[0, 0, -1, 0, 1, 0, 1, 0, 0], idle_position] # self.idle_T=self.end_T # self.sim.controller(0).setPIDCommand(self.idleq,[0]*7) [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 1) # constraints=ik.objective(robot.link(ee_link),local=local_p,world=world_p) # traj1 = cartesian_trajectory.cartesian_interpolate_linear(robot,self.start_T,self.end_T,constraints,delta=1e-2,maximize=False) # print 'traj1:',traj1[0] else: # print 'idle', robot.getConfig() self.set_state('find_target') elif self.state == 'find_target': h = 0 for i in self.waiting_list: if self.sim.world.rigidObject(i).getTransform()[1][2] > h: self.target = i h = self.sim.world.rigidObject(i).getTransform()[1][2] print 'target is ', self.target # self.target=0 # self.object_p=self.sim.world.rigidObject(self.target).getTransform()[1] bb1, bb2 = self.sim.world.rigidObject( self.target).geometry().getBB() self.object_p = vectorops.div(vectorops.add(bb1, bb2), 2) # print 'object xform', self.sim.world.rigidObject(self.target).getTransform()[1] # print 'object_p',self.object_p h = self.object_p[2] + box_vacuum_offset[2] if self.object_p[1] > shelf_position[1]: end_p = approach_p1 end_p[2] = h else: end_p = approach_p2 end_p[2] = h d = vectorops.distance(end_p[:1], self.object_p[:1]) dx = self.object_p[0] - end_p[0] dy = self.object_p[1] - end_p[1] self.end_T = [[0, 0, -1, -dy / d, dx / d, 0, dx / d, dy / d, 0], end_p] print 'approach start position', end_p self.retract_T = [[ 0, 0, -1, -dy / d, dx / d, 0, dx / d, dy / d, 0 ], end_p] print 'retract_T', self.retract_T self.set_state('preposition') elif self.state == 'preposition': t = 0.5 if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 1) else: self.end_T[1] = vectorops.add(self.object_p, [0.10, 0, 0.12]) self.set_state('pregrasp') elif self.state == 'pregrasp': t = 0.5 # print 'pregrasp' if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 1) else: self.end_T[1][2] -= 0.03 self.set_state('grasp') elif self.state == 'grasp': # print 'grasp' t = 0.5 if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 1) else: self.end_T[1][2] += 0.03 self.set_state('prograsp') elif self.state == 'prograsp': # print 'grasp' self.graspedObjects[self.target] = True t = 0.5 if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 1) else: self.end_T = self.retract_T self.end_T[1][2] += 0.01 self.set_state('retract') elif self.state == 'retract': # print 'retract' t = 0.5 if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 1) else: self.set_state('go_to_tote') elif self.state == 'go_to_tote': x = robot.link(ee_link).getTransform()[1][0] if x > (order_box_min[0] + order_box_max[0]) / 2: q = robot.getConfig() q[1] += 0.02 # self.sim.controller(0).setPIDCommand(q,[0]*7) robotController.setLinear(q, 0.001) # self.output.append(vectorops.mul(q[1:],angle_to_degree)) else: bb1, bb2 = self.sim.world.rigidObject( self.target).geometry().getBB() bbx = bb2[0] - bb1[0] bby = bb2[1] - bb1[1] print 'BB:', bbx, bby # self.end_T[1]=[order_box_min[0]+bbx/2-0.01,order_box_min[1]+bby/2+0.21-self.target*0.1,0.8] self.end_T[1] = self.find_placement() # if self.score>0: # pass self.end_T[0] = [0, 0, -1, -1, 0, 0, 0, 1, 0] self.set_state('place') elif self.state == 'place': t = 0.5 if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 0) else: self.end_T[1][2] -= 0.01 self.set_state('release') elif self.state == 'release': t = 0.5 if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 0) else: self.graspedObjects[self.target] = False self.check_target() self.set_state('idle') old_T = robot.link(ee_link).getTransform() old_R, old_t = old_T if local_p: q_old = robot.getConfig() goal = ik.objective(robot.link(ee_link), local=local_p, world=world_p) # s=ik.solver(goal) # s.setMaxIters(100) # s.setTolerance(0.2) # if s.solve(): # pass # else: # print "IK failed" # while not s.solve(): # q_restart=[0,0,0,0,0,0,0] # q=robot.getConfig() # (qmin,qmax) = robot.getJointLimits() # for i in range(7): # q_restart[i]=random.uniform(-1,1)+q[i] # q_restart[i] = min(qmax[i],max(q_restart[i],qmin[i])) # print q_restart # robot.setConfig(q_restart) # if s.solve(): # print "IK solved" # else: # print "IK failed" # s=ik.solve_global(goal) s = ik.solve_nearby(goal, maxDeviation=10, feasibilityCheck=test_function) q = robot.getConfig() if not feasible(robot, q, q_old): s = ik.solve_global(goal) q = robot.getConfig() robotController = self.sim.controller(0) qdes = q (qmin, qmax) = robot.getJointLimits() for i in xrange(len(qdes)): qdes[i] = min(qmax[i], max(qdes[i], qmin[i])) # robotController.setPIDCommand(qdes,rvels[0]) robotController.setLinear(qdes, 0.001) # self.output.append(vectorops.mul(qdes[1:],angle_to_degree)) obj = self.sim.world.rigidObject(self.target) #get a SimBody object body = self.sim.body(obj) if self.graspedObjects[self.target]: T = body.getTransform() R, t = T body.enableDynamics(False) if self.flag == 0: self.flag = 1 self.t = robot.link(ee_link).getLocalPosition(t) # print 'here' new_T = robot.link(ee_link).getTransform() new_R, new_t = new_T change_R = so3.mul(new_R, so3.inv(old_R)) R = so3.mul(change_R, R) body.setTransform(R, robot.link(ee_link).getWorldPosition(self.t)) else: body.enableDynamics(True) self.flag = 0 return
def so3_staggered_grid(N): """Returns a nx.Graph describing a staggered grid on SO3 with resolution N. The resulting graph has ~8N^3 nodes The node attribute 'params' gives the so3 element. """ import itertools assert N >= 1 G = nx.Graph() R0 = so3.from_quaternion(vectorops.unit((1, 1, 1, 1))) #R0 = so3.identity() namemap = dict() for ax in range(4): for i in xrange(N + 1): u = 2 * float(i) / N - 1.0 u = math.tan(u * math.pi * 0.25) u2 = 2 * float(i + 0.5) / N - 1.0 u2 = math.tan(u2 * math.pi * 0.25) for j in xrange(N + 1): v = 2 * float(j) / N - 1.0 v = math.tan(v * math.pi * 0.25) v2 = 2 * float(j + 0.5) / N - 1.0 v2 = math.tan(v2 * math.pi * 0.25) for k in xrange(N + 1): w = 2 * float(k) / N - 1.0 w = math.tan(w * math.pi * 0.25) w2 = 2 * float(k + 0.5) / N - 1.0 w2 = math.tan(w2 * math.pi * 0.25) idx = [i, j, k] idx = idx[:ax] + [N] + idx[ax:] idx = tuple(idx) if idx in namemap: pass else: namemap[idx] = idx flipidx = tuple([N - x for x in idx]) namemap[flipidx] = idx q = [u, v, w] q = q[:ax] + [1.0] + q[ax:] q = vectorops.unit(q) R = so3.mul(so3.inv(R0), so3.from_quaternion(q)) G.add_node(idx, params=R) if i < N and j < N and k < N: #add staggered point sidx = [i + 0.5, j + 0.5, k + 0.5] sidx = sidx[:ax] + [N] + sidx[ax:] sidx = tuple(sidx) sfidx = tuple([N - x for x in sidx]) namemap[sidx] = sidx namemap[sfidx] = sidx q = [u2, v2, w2] q = q[:ax] + [1.0] + q[ax:] q = vectorops.unit(q) R = so3.mul(so3.inv(R0), so3.from_quaternion(q)) G.add_node(sidx, params=R) for ax in range(4): for i in xrange(N + 1): for j in xrange(N + 1): for k in xrange(N + 1): baseidx = [i, j, k] idx = baseidx[:ax] + [N] + baseidx[ax:] idx = tuple(idx) for n in range(3): nidx = baseidx[:] nidx[n] += 1 if nidx[n] > N: continue nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) G.add_edge(namemap[idx], namemap[nidx]) #edges between staggered point and grid points baseidx = [i + 0.5, j + 0.5, k + 0.5] if any(v > N for v in baseidx): continue idx = baseidx[:ax] + [N] + baseidx[ax:] idx = tuple(idx) for ofs in itertools.product(*[(-0.5, 0.5)] * 3): nidx = vectorops.add(baseidx, ofs) nidx = [int(x) for x in nidx] nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) #print "Stagger-grid edge",idx,nidx G.add_edge(namemap[idx], namemap[nidx]) #edges between staggered points -- if it goes beyond face, #go to the adjacent face for n in range(3): nidx = baseidx[:] nidx[n] += 1 if nidx[n] > N: #swap face nidx[n] = N nidx = nidx[:ax] + [N - 0.5] + nidx[ax:] nidx = tuple(nidx) #if not G.has_edge(namemap[idx],namemap[nidx]): # print "Stagger-stagger edge",idx,nidx,"swap face" else: nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) #if not G.has_edge(namemap[idx],namemap[nidx]): # print "Stagger-stagger edge",idx,nidx G.add_edge(namemap[idx], namemap[nidx]) return G
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, SimRobotSensor 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 xrange(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, prefix + "/" + world.rigidObject(i).getName(), root) for i in xrange(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 xrange(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, rprefix + "/" + robot.link(j).getName(), root) 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(j).getName(), rprefix + "/" + robot.link(p).getName()) return if isinstance(klampt_obj, SimRobotSensor): from ..model import sensing Tsensor_link = sensing.get_sensor_xform(klampt_obj) link = int(klampt_obj.getSetting('link')) if klampt_obj.type( ) == 'LaserRangeSensor': #the convention between Klampt and ROS is different klampt_to_ros_lidar = so3.from_matrix([[0, 1, 0], [0, 0, 1], [1, 0, 0]]) Tsensor_link = (so3.mul(Tsensor_link[0], klampt_to_ros_lidar), Tsensor_link[1]) q = so3.quaternion(Tsensor_link[0]) robot = klampt_obj.robot() broadcaster.sendTransform( Tsensor_link[1], (q[1], q[2], q[3], q[0]), stamp, frameprefix + "/" + robot.getName() + '/' + klampt_obj.name(), frameprefix + "/" + robot.link(link).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, frameprefix, root) return