def getEnv(self, key, iter=0, first=False): val = self.base.getModel(key) print key, val obj = None obj_list = [] parent_tf = tf.identity_matrix() if self.base.typeOf(key) == self.base.ROBOTS.ident: obj = self._getRobot(key) obj_list.append(obj) parent_tf = obj.GetTransform() elif self.base.typeOf(key) == self.base.OBJECTS.ident: obj = self._getObject(key) obj_list.append(obj) parent_tf = obj.GetTransform() elif self.base.typeOf(key) == self.base.LOCATIONS.ident: obj = self._getLocation(key) obj_list.append(obj) parent_tf = obj.GetTransform() elif self.base.typeOf(key) == self.base.SENSORS.ident: obj = self._getSensor(key) obj_list.append(obj) parent_tf = obj.GetTransform() else: if val.has_key("translation"): translation = map(lambda x: float(x), val["translation"].split(" ")) parent_tf = tf.concatenate_matrices(parent_tf, tf.compose_matrix(translate = translation)) if val.has_key("quat"): quat = map(lambda x: float(x), val["quat"].split(" ")) rot = rave.axisAngleFromQuat(quat) m2 = tf.compose_matrix(angles = rot) parent_tf = tf.concatenate_matrices(parent_tf, m2) if first: parent_tf = tf.identity_matrix() if obj != None: obj.SetTransform(parent_tf) if iter==0: return obj_list # search for ancestors child_expr = pycassa.create_index_expression('base', key) clild_clause = pycassa.create_index_clause([child_expr]) for child_key, _ in self.base.col.get_indexed_slices(clild_clause): child_obj = self.getEnv(child_key, iter-1) for obj in child_obj: if type(obj) != type(None): obj.SetTransform(tf.concatenate_matrices(parent_tf, obj.GetTransform())) obj_list.append(obj) return obj_list
def motion_model(self, f0, f1, stamp, use_kalman=False): if not use_kalman: # simple `repetition` model txn0, rxn0 = f0['pose'][L_POS], f0['pose'][A_POS] txn1, rxn1 = f1['pose'][L_POS], f1['pose'][A_POS] R0 = tx.euler_matrix(*rxn0) R1 = tx.euler_matrix(*rxn1) T0 = tx.compose_matrix(angles=rxn0, translate=txn0) T1 = tx.compose_matrix(angles=rxn1, translate=txn1) Tv = np.dot(T1, vm.inv(T0)) # Tv * T0 = T1 T2 = np.dot(Tv, T1) txn = tx.translation_from_matrix(T2) rxn = tx.euler_from_matrix(T2) x = f1['pose'].copy() P = f1['cov'].copy() x[0:3] = txn x[9:12] = rxn return x, P else: # dt MUST NOT BE None self.kf_.x = f0['pose'] self.kf_.P = f0['cov'] dt = (f1['stamp'] - f0['stamp']) self.kf_.predict(dt) txn, rxn = f1['pose'][L_POS], f1['pose'][A_POS] z = np.concatenate([txn, rxn]) self.kf_.update(z) dt = (stamp - f1['stamp']) self.kf_.predict(dt) return self.kf_.x.copy(), self.kf_.P.copy()
def pose_callback(self, msg): print "pose cb" trans = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z] rot = [ msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w ] pose_matrix = tr.compose_matrix(angles=tr.euler_from_quaternion(rot), translate=trans) # Transfer to the frame of the robot # pose_matrix = np.dot(self.config, pose_matrix) if self.previous_tf is None: #if first callback odo_trans = trans odo_rot = rot odo = tr.compose_matrix(angles=tr.euler_from_quaternion(odo_rot), translate=odo_trans) self.origin = trans else: # calculate limit matrix if enable_filter: limit_dist = limit_speed * (msg.header.stamp.to_nsec() - self.previous_time.to_nsec()) print(msg.header.stamp.to_nsec() - self.previous_time.to_nsec()) scale, shear, angles, prev_trans, persp = tr.decompose_matrix( self.previous_tf) moved_vec = [ msg.pose.position.x - prev_trans[0], msg.pose.position.y - prev_trans[1], msg.pose.position.z - prev_trans[2] ] moved_dist = np.linalg.norm(moved_vec) if moved_dist > limit_dist: #discard this pose print "move too much" return odo = np.dot(tr.inverse_matrix(self.previous_tf), pose_matrix) odo_trans = tf.transformations.translation_from_matrix(odo) odo_rot = tf.transformations.quaternion_from_matrix(odo) self.previous_time = msg.header.stamp self.previous_tf = pose_matrix print "x: ", trans[0] - self.origin[0], "y: ", trans[1] - self.origin[ 1], "z: ", trans[2] - self.origin[2] robot_odo = PoseStamped() robot_odo.header.stamp = msg.header.stamp robot_odo.pose.position.x = odo_trans[0] robot_odo.pose.position.y = odo_trans[1] robot_odo.pose.position.z = odo_trans[2] robot_odo.pose.orientation.x = odo_rot[0] robot_odo.pose.orientation.y = odo_rot[1] robot_odo.pose.orientation.z = odo_rot[2] robot_odo.pose.orientation.w = odo_rot[3] self.pub_odom.publish(robot_odo)
def updateStoredTransforms(update): global holoTransformList global markerTransformList global worldTransform global offsetTransform arrSize = holoTransformList.shape[0] #parse input into meaningful values xworldT = update[0:3] xworldR = update[3:6] xworldRQ = TR.quaternion_about_axis(np.sqrt(xworldR.dot(xworldR)), xworldR) xoffsetT = update[6:9] xoffsetR = update[9:12] xoffsetRQ = TR.quaternion_about_axis(np.sqrt(xoffsetR.dot(xoffsetR)), xoffsetR) #update world transform worldIncrement = TR.compose_matrix( translate=xworldT, angles=TR.euler_from_quaternion(xworldRQ)) worldTransform = np.dot(worldIncrement, worldTransform) #update offset transform offsetIncrement = TR.compose_matrix( translate=xoffsetT, angles=TR.euler_from_quaternion(xoffsetRQ)) offsetTransform = np.dot(offsetIncrement, offsetTransform)
def point_cb(self, msg): print "point cb" trans = [ self.pose_msg.pose.position.x / 1000, self.pose_msg.pose.position.y / 1000, self.pose_msg.pose.position.z / 1000 ] rot = [ self.pose_msg.pose.orientation.x, self.pose_msg.pose.orientation.y, self.pose_msg.pose.orientation.z, self.pose_msg.pose.orientation.w ] pose_matrix = tr.compose_matrix(angles=tr.euler_from_quaternion(rot), translate=trans) pt_trans = [msg.point.x, msg.point.y, msg.point.z] pt_rot = [0, 0, 0, 1] pt_matrix = tr.compose_matrix(angles=tr.euler_from_quaternion(pt_rot), translate=pt_trans) obj_pose = np.dot(pose_matrix, pt_matrix) obj_position = tf.transformations.translation_from_matrix(obj_pose) print "pose = ", self.pose_msg.pose print "point = ", msg.point print "obj position = ", obj_position point_msg = PointStamped() point_msg.header.stamp = rospy.Time() point_msg.header.seq = msg.header.seq point_msg.header.frame_id = "map" point_msg.point.x = obj_position[0] point_msg.point.y = obj_position[1] point_msg.point.z = obj_position[2] self.pub_obj_position.publish(point_msg)
def addTransforms(self, frame1, frame2): """ Adds two transform objects to get a resulting transform. """ t1 = np.array( [frame1.translation.x, frame1.translation.y, frame1.translation.z]) t2 = np.array( [frame2.translation.x, frame2.translation.y, frame2.translation.z]) r1 = np.array([ frame1.rotation.x, frame1.rotation.y, frame1.rotation.z, frame1.rotation.w ]) r2 = np.array([ frame2.rotation.x, frame2.rotation.y, frame2.rotation.z, frame2.rotation.w ]) f1 = t.compose_matrix(translate=t1, angles=t.euler_from_quaternion(r1)) f2 = t.compose_matrix(translate=t2, angles=t.euler_from_quaternion(r2)) f = np.matmul(f1, f2) trans = t.translation_from_matrix(f) rot = t.quaternion_from_matrix(f) trans = Vector3(trans[0], trans[1], trans[2]) rot = Quaternion(rot[0], rot[1], rot[2], rot[3]) return Transform(trans, rot)
def angle_diff_from_quaternions(q1, q2): tf1 = transformations.compose_matrix( angles=transformations.euler_from_quaternion(q1)) tf2 = transformations.compose_matrix( angles=transformations.euler_from_quaternion(q2)) angle, _, _ = transformations.rotation_from_matrix( np.dot(np.linalg.inv(tf1), tf2)) return angle / np.pi * 180.
def _get_tf_matrix(self, pose): if isinstance(pose, dict): _quat = np.array([pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w]) _trans = np.array([pose['position'].x, pose['position'].y, pose['position'].z]) return tr.compose_matrix(angles=tr.euler_from_quaternion(_quat,'sxyz'), translate=_trans) else: _quat = np.array([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]) _trans = np.array([pose.position.x, pose.position.y, pose.position.z]) return tr.compose_matrix(angles=tr.euler_from_quaternion(_quat,'sxyz'), translate=_trans)
def transform_pose_to_base_link(self, t, q): euler_pose = tfm.euler_from_quaternion(q) tf_cam_col_opt_fram = tfm.compose_matrix(translate=t, angles=euler_pose) trans, quat = self.listener.lookupTransform( 'base_link', 'camera_color_optical_frame', rospy.Time(0)) euler = tfm.euler_from_quaternion(quat) tf = tfm.compose_matrix(translate=trans, angles=euler) t_pose = np.dot(tf, tf_cam_col_opt_fram) t_ba_li = t_pose[0:3, 3] return t_ba_li
def transformOpenrave(self, openrave_obj, complex_data): if complex_data.has_key("translation"): translation = map(lambda x: float(x), complex_data["translation"].split(" ")) m1 = openrave_obj.GetTransform() m2 = tf.compose_matrix(translate = translation) openrave_obj.SetTransform(tf.concatenate_matrices(m1, m2)) if complex_data.has_key("quat"): quat = map(lambda x: float(x), complex_data["quat"].split(" ")) m1 = openrave_obj.GetTransform() rot = rave.axisAngleFromQuat(quat) m2 = tf.compose_matrix(angles = rot) openrave_obj.SetTransform(tf.concatenate_matrices(m1, m2)) return openrave_obj
def controller(): rospy.sleep(1) print("en el controler") k_v = 0.4 k_w = 1 x_goal = -3 y_goal = 2 theta_goal = 0 while not rospy.is_shutdown(): print("true goal:", x_goal, y_goal) (roll_bot, pitch_bot, yaw_bot) = euler_from_quaternion([ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ]) orient = np.array([roll_bot, pitch_bot, yaw_bot]) trans = np.array([pose.position.x, pose.position.y, 0]) T_bot = compose_matrix(angles=orient, translate=trans) T_bot_inv = np.linalg.inv(T_bot) orient = np.array([0, 0, 0]) trans = np.array([x_goal, y_goal, 0]) T_punto = compose_matrix(angles=orient, translate=trans) T_punto_bot = np.matmul(T_bot_inv, T_punto) punto_x = T_punto_bot[0][3] punto_y = T_punto_bot[1][3] print("goal from bot:", punto_x, punto_y) giro = atan2(punto_y, punto_x) print("angulo_a_girar: ", degrees(giro)) print("\n") if ((sqrt((pose.position.x - x_goal)**2 + (pose.position.y - y_goal)**2)) < 0.01): twist.linear.x = 0 twist.angular.z = 0 else: twist.angular.z = k_w * (giro) twist.linear.x = k_v * sqrt((pose.position.x - x_goal)**2 + (pose.position.y - y_goal)**2) if (twist.linear.x > 0.3): #control velocidad twist.linear.x = 0.3 if (twist.angular.z > 1): #control velocidad angular twist.angular.z = 1 if (twist.angular.z < (-1)): #control velocidad angular twist.angular.z = 1 * -1 twist_pub.publish(twist) print(twist) rate.sleep()
def controller(): rospy.sleep(1) print("en el controler") k_v=1;k_w=1; # path=[[-0.6,0],[-3.5,0],[-3.5,3.5],[1.5,3.5],[1.5,-1.5],[3.5,-1.5],[3.5,-8.0],[-2.5,-8.0],[-2.5,-5.5],[1.5,-5.5],[1.5,-3.5],[-1,-3.5]] # x_goal=path[0][0];y_goal=path[0][1]; x_goal=traj.poses.pop(0).pose.position.x;y_goal=path[0][1]; next=0 while not rospy.is_shutdown(): print("true goal:", x_goal, y_goal) (roll_bot,pitch_bot,yaw_bot)=euler_from_quaternion([pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w]) trans = np.array([pose.position.x,pose.position.y,0]) orient = np.array([roll_bot,pitch_bot,yaw_bot]) T_bot = compose_matrix(angles=orient, translate=trans) T_bot_inv=np.linalg.inv(T_bot) trans = np.array([x_goal,y_goal,0]) orient = np.array([0,0,0]) T_punto = compose_matrix(angles=orient, translate=trans) T_punto_bot=np.matmul(T_bot_inv,T_punto) punto_x=T_punto_bot[0][3];punto_y=T_punto_bot[1][3] # print("goal from bot:", punto_x, punto_y) giro=atan2(punto_y,punto_x) # print("angulo_a_girar: ", degrees(giro)) # print("\n") if ((sqrt((pose.position.x-x_goal)**2+(pose.position.y-y_goal)**2))<0.5): if (not next>=len(path)): x_goal=path[next][0];y_goal=path[next][1]; # x_goal=traj.poses.pose.point.x;y_goal=traj.poses.pose.point.y next=next+1 else: if ((sqrt((pose.position.x-x_goal)**2+(pose.position.y-y_goal)**2))<0.05): giro=0;pose.position.x=x_goal;pose.position.y=y_goal; twist.angular.z=k_w*(giro) twist.linear.x=k_v*sqrt((pose.position.x-x_goal)**2+(pose.position.y-y_goal)**2) if(twist.linear.x>0.3): #control velocidad lineal twist.linear.x=0.3 if(twist.angular.z>radians(60)): #control velocidad angular twist.angular.z=radians(60) if(twist.angular.z<(radians(-60))): #control velocidad angular twist.angular.z=radians(-60) twist_pub.publish(twist) # print(twist) rate.sleep()
def controller(self): rospy.sleep(1) print("en el controler") k_v=1;k_w=1; punto=self.traj.poses.pop(0).pose; x_goal=punto.position.x y_goal=punto.position.y self.new=False while not rospy.is_shutdown() and not self.new: print("true goal:", x_goal, y_goal) (roll_bot,pitch_bot,yaw_bot)=euler_from_quaternion([pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w]) trans = np.array([pose.position.x,pose.position.y,0]) orient = np.array([roll_bot,pitch_bot,yaw_bot]) T_bot = compose_matrix(angles=orient, translate=trans) T_bot_inv=np.linalg.inv(T_bot) trans = np.array([x_goal,y_goal,0]) orient = np.array([0,0,0]) T_punto = compose_matrix(angles=orient, translate=trans) T_punto_bot=np.matmul(T_bot_inv,T_punto) punto_x=T_punto_bot[0][3];punto_y=T_punto_bot[1][3] print("goal from bot:", punto_x, punto_y) giro=atan2(punto_y,punto_x) print("angulo_a_girar: ", degrees(giro)) # print("\n") if ((sqrt((pose.position.x-x_goal)**2+(pose.position.y-y_goal)**2))<0.5): if (len(self.traj.poses)>0): punto=self.traj.poses.pop(0).pose; x_goal=punto.position.x y_goal=punto.position.y else: if ((sqrt((pose.position.x-x_goal)**2+(pose.position.y-y_goal)**2))<0.01): giro=0;pose.position.x=x_goal;pose.position.y=y_goal; twist.angular.z=k_w*(giro) twist.linear.x=k_v*sqrt((pose.position.x-x_goal)**2+(pose.position.y-y_goal)**2) if(twist.linear.x>0.3): #control velocidad lineal twist.linear.x=0.3 if(twist.angular.z>radians(60)): #control velocidad angular twist.angular.z=radians(60) if(twist.angular.z<(radians(-60))): #control velocidad angular twist.angular.z=radians(-60) twist_pub.publish(twist) print(twist) rate.sleep()
def get_kinematic_chain(self, target, source, reference): """ Gets the pose for all joints in the kinematic chain from source to target. :param target: name of target joint :param source : name of source joint :param reference: name of reference joint as needed by ros function listener.chain :type target: string :type source: string :type reference: string :return: kinematic chain, transforms as translation and quaternions, transforms as matrices :rtype: list of strings, list of transforms, list of numpy matrices """ # All frames here. listener = self.listener chain = listener.chain(target, rospy.Time(), source, rospy.Time(), reference) Ts, TMs = [], [] np.set_printoptions(precision=4, suppress=True) TMcum = np.eye(4, 4) for i in range(len(chain) - 1): t = listener.lookupTransform(chain[i + 1], chain[i], rospy.Time()) t = [[np.float64(_) for _ in t[0]], [np.float64(_) for _ in t[1]]] t1_euler = tfx.euler_from_quaternion(t[1]) tm = tfx.compose_matrix(translate=t[0], angles=t1_euler) Ts.append(t) TMs.append(tm) t = listener.lookupTransform(chain[i + 1], chain[0], rospy.Time()) t1_euler = tfx.euler_from_quaternion(t[1]) tm = tfx.compose_matrix(translate=t[0], angles=t1_euler) TMcum = np.dot(TMs[i], TMcum) eye = np.dot(tm, np.linalg.inv(TMcum)) assert (np.allclose(eye - np.eye(4, 4), np.zeros((4, 4)), atol=0.1)) # Sanity check to make sure we understand what is happening here. # for i in range(len(chain)-1): # t = listener.lookupTransform(chain[i+1], chain[0], rospy.Time(0)) # tm = tfx.compose_matrix(translate=t[0], angles=t[1]) # TMcum = np.dot(TMs[i], TMcum) # eye = np.dot(tm, np.linalg.inv(TMcum)) # print(eye-np.eye(4,4)) # assert(np.allclose(eye-np.eye(4,4), np.zeros((4,4)), atol=1e-2)) return chain, Ts, TMs
def parseUrdfJoint(xml, jointNames, jonintTree): '''获取每个关节的实际位姿''' matrix = OrderedDict() result = OrderedDict() qmatrix = OrderedDict() # axises=[] #获取关节参数 for jname in jointNames: jointNode = xml.xpath("//joint[@name='%s']" % jname)[0] originNode = jointNode.find("origin") axisNode = jointNode.find("axis") origin_xyz = [0.0, 0.0, 0.0] if originNode is None else map( float, originNode.attrib['xyz'].split()) origin_rpy = [0.0, 0.0, 0.0] if originNode is None else map( float, originNode.attrib['rpy'].split()) # axis_xyz=[0.0,0.0,1.0] if axisNode is None else map(float,axisNode.attrib['xyz'].split()) mat = compose_matrix(angles=origin_rpy, translate=origin_xyz) pname = getParentJointName(jonintTree, jname) #获取当前关节的父关节名称 if pname is None: matrix[jname] = mat else: matrix[jname] = matrix[pname].dot(mat) scale, shear, angles, translate, perspective = decompose_matrix( matrix[jname]) result[jname] = [ np.array(translate).round(8), np.array(angles).round(8) ] qmatrix[jname] = [ translate.round(8), quaternion_from_euler(*angles).round(8) ] # axises.append(axis_xyz) return result
def __init__(self, robot, remove_object_model_func): Task2State.__init__( self, robot, outcomes=['finish', 'continue'], input_keys=['object_count', 'orig_channel_xy_yaw'], output_keys=['object_count', 'orig_channel_xy_yaw']) self.remove_object_model_func = remove_object_model_func self.global_place_channel_z = rospy.get_param( '~global_place_channel_z') self.global_place_channel_yaw = rospy.get_param( '~global_place_channel_yaw') self.place_z_margin = rospy.get_param('~place_z_margin') self.place_z_offset = rospy.get_param('~place_z_offset') self.object_num = rospy.get_param('~object_num') self.skip_ungrasp = rospy.get_param('~skip_ungrasp', False) self.do_channel_recognition = rospy.get_param( '~do_channel_recognition') self.single_object_mode = rospy.get_param('~single_object_mode') grasping_point = rospy.get_param('~grasping_point') self.grasping_yaw = rospy.get_param('~grasping_yaw') self.grasping_point_coords = tft.compose_matrix( translate=[ grasping_point[0], grasping_point[1], grasping_point[2] ], angles=[0, 0, self.grasping_yaw])
def trajectory(self, node: str) -> List[Dict[str, List]]: # collect all timed nodes corresponding to the node to track traj = [] for nname, ndata in self._graph.nodes(data=True): if ndata['__name__'] == node: # Filter out Duckiebot nodes that are not connected to # another Duckiebot node dbot_type = AutolabReferenceFrame.TYPE_DUCKIEBOT_FOOTPRINT if ndata["type"] == dbot_type: if not self._graph.has_neighbor_of_type(nname, dbot_type): continue T = tr.compose_matrix(translate=ndata["pose"].t, angles=tr.euler_from_quaternion( ndata["pose"].q)) traj.append({ 'timestamp': ndata["time"], 'pose': T.tolist(), # TODO: this is a hack, should be done properly 'observers': list({ p[0] for e in self._graph.out_edges(nname) for p in self._graph.in_edges(e[1]) } if 'footprint' in nname else {}) }) # sort trajectory by time strategy = lambda tf: tf['timestamp'] traj = sorted(traj, key=strategy) # --- return traj
def load_config(self): anchor_data = yaml.load(file(rospkg.RosPack().get_path('odom_uwb')+"/config/"+"husky.yaml",'r')) # Need RosPack get_path to find the file path trans = anchor_data['trans'] rot = anchor_data['rot'] return tr.compose_matrix(angles = rot, translate = trans)
def filterPointCloud(bin_num, pts, source_pc2_kinect_header, height=None, width=None, retPointCloud=True): # input height/width if you need filtered uv with Timer('b1'): global tfListener source_pc_kinect = PointCloud() source_pc_kinect.header = source_pc2_kinect_header with Timer('b2'): source_pc_kinect.points = pts # transform point cloud from kinect frmae to shelf frame with Timer('b3'): import tf.transformations as tfm import numpy as np (pos, ori) = tfListener.lookupTransform('/shelf', source_pc_kinect.header.frame_id, source_pc_kinect.header.stamp) points = [] T = np.dot(tfm.compose_matrix(translate=pos) , tfm.quaternion_matrix(ori) ) for pt in pts: #tmp = np.dot(T, np.array(pt+[1])).tolist() points.append( np.dot(T, np.array(pt+[1])).tolist() ) pts = [] cnt = 0 with Timer('b4'): if height and width: filtered_uvs = [] filtered_uvs_mask = [False for i in xrange(height*width)] for point in points: # change source to source_pc_kinect if ~math.isnan(point[0]) and inside_bin(point, bin_num): pts.append(point) # can be removed becuase we only need mask filtered_uvs_mask[cnt] = True filtered_uvs.append([cnt//width, cnt%width]) cnt += 1 else: for point in points: # change source to source_pc_kinect if inside_bin(point, bin_num): pts.append(point) if retPointCloud: with Timer('b5'): filtered_pc_shelf = PointCloud() filtered_pc_shelf.header = source_pc2_kinect_header filtered_pc_shelf.header.frame_id = '/shelf' filtered_pc_shelf.points = [Point32(pt[0], pt[1], pt[2]) for pt in pts] # render filtered point cloud filtered_pc_map = tfListener.transformPointCloud('/map', filtered_pc_shelf) createPointsMarker(filtered_pc_map.points, marker_id=2, frame_id='/map', rgba=(0,1,0,1)) #points are in Point32 with Timer('b6'): if height and width: if retPointCloud: return (filtered_pc_map, filtered_uvs, filtered_uvs_mask) else: return filtered_uvs_mask else: return filtered_pc_map
def clickedCB(data): global askedMarkers global listener global worldTransform global measuredMarkers global clickNum print 'clicked' try: (tipTrans, tipRot) = listener.lookupTransform('/mocha_world', '/tip', rospy.Time(0)) measuredMarkers[clickNum, :] = tipTrans clickNum = clickNum + 1 if clickNum == 8: clickNum = 0 print 'finished clicking' print 'AskedMarkers:' print askedMarkers print 'measured markers:' print measuredMarkers (worldRotupdate, worldTransUpdate) = rigid_transform_3D(measuredMarkers, askedMarkers) worldRot4 = np.identity(4) for i in range(3): worldRot4[(0, 1, 2), i] = worldRotupdate[:, i] worldTransformUpdate = TR.compose_matrix( translate=worldTransUpdate, angles=TR.euler_from_matrix(worldRot4)) worldTransformationUpdateInv = TR.inverse_matrix( worldTransformUpdate) worldTransform = np.dot(worldTransformationUpdateInv, worldTransform) except (): nothing = 0
def __init__(self, robot, add_object_model_func): Task2State.__init__(self, robot, outcomes=['succeeded', 'failed'], input_keys=['object_count'], output_keys=['object_count']) self.add_object_model_func = add_object_model_func self.do_object_recognition = rospy.get_param('~do_object_recognition') self.joint_torque_thresh = rospy.get_param('~joint_torque_thresh') self.skip_grasp = rospy.get_param('~skip_grasp', False) self.object_grasping_height = rospy.get_param( '~object_grasping_height') self.object_interval = rospy.get_param('~object_interval') self.lane_interval = rospy.get_param('~lane_interval') self.global_first_object_pos = rospy.get_param( '~global_first_object_pos') self.global_object_yaw = rospy.get_param('~global_object_yaw') self.grasp_land_mode = rospy.get_param('~grasp_land_mode') self.reset_realsense_odom = rospy.get_param('~reset_realsense_odom') self.stop_if_grasp_failed = rospy.get_param('~stop_if_grasp_failed') self.reset_realsense_client = rospy.ServiceProxy( '/realsense1/odom/reset', std_srvs.srv.Empty) grasping_point = rospy.get_param('~grasping_point') grasping_yaw = rospy.get_param('~grasping_yaw') self.grasping_point_coords = tft.compose_matrix( translate=[ grasping_point[0], grasping_point[1], grasping_point[2] ], angles=[0, 0, grasping_yaw]) self.grasp_force_landing_mode = rospy.get_param( '~grasp_force_landing_mode') self.force_landing_height = rospy.get_param('~force_landing_height')
def ref_pose_cb(self, some_pose): # Takes target pose, returns ref se3 rospy.logdebug("ref_pose_cb called in velocity_control.py") p = np.array([some_pose.position.x, some_pose.position.y, some_pose.position.z]) quat = np.array([some_pose.orientation.x, some_pose.orientation.y, some_pose.orientation.z, some_pose.orientation.w]) goal_tmp = tr.compose_matrix(angles=tr.euler_from_quaternion(quat, 'sxyz'), translate=p) with self.mutex: self.T_goal = goal_tmp
def __init__(self, robot): Task2State.__init__( self, robot, outcomes=['succeeded', 'failed', 'adjust_again'], input_keys=['orig_global_trans', 'search_count', 'search_failed'], output_keys=['orig_global_trans', 'search_count', 'search_failed']) self.skip_adjust_grasp_position = rospy.get_param( '~skip_adjust_grasp_position', False) self.do_object_recognition = rospy.get_param('~do_object_recognition') self.object_yaw_thresh = rospy.get_param('~object_yaw_thresh') self.global_object_yaw = rospy.get_param('~global_object_yaw') self.grasping_yaw = rospy.get_param('~grasping_yaw') self.recognition_wait = rospy.get_param('~recognition_wait') self.adjust_grasp_image_type = rospy.get_param( '~adjust_grasp_image_type', 'depth') self.object_pos_thresh = rospy.get_param('~object_pos_thresh') grasping_point = rospy.get_param('~grasping_point') grasping_yaw = rospy.get_param('~grasping_yaw') self.grasping_point_coords = tft.compose_matrix( translate=[ grasping_point[0], grasping_point[1], grasping_point[2] ], angles=[0, 0, grasping_yaw]) self.object_pose_sub = rospy.Subscriber( 'rectangle_detection_' + self.adjust_grasp_image_type + '/target_object_' + self.adjust_grasp_image_type, PoseArray, self.objectPoseCallback) self.object_pose = PoseArray()
def read_pos(t, link_states, recorder_pos, joint_states, readLinkState): #pub_foot_z, pub_body_z): ## Initialization from geometry_msgs.msg import Wrench, Vector3 from std_msgs.msg import Float64 import rospy import math from numpy.linalg import norm from gazebo_msgs.srv import GetLinkState from tf.transformations import euler_from_quaternion, quaternion_from_euler, compose_matrix, quaternion_matrix if readLinkState.value is None: readLinkState.value = rospy.ServiceProxy("/gazebo/get_link_state", GetLinkState) ## set time from which recording of data (for CSV) should start record_time = 0.0 # get current angle phi1 = joint_states.value.position[1] phi3 = -joint_states.value.position[3] ## definitions according to matlab script/ RAL paper, defined solely for comparison offset_q1 = -40*math.pi/180 offset_q2 = 40*math.pi/180 q1 = phi1+offset_q1 q3 = phi3+offset_q2 body_pos = readLinkState.value('base_link', 'link') body_z = body_pos.link_state.pose.position.z body_x = body_pos.link_state.pose.position.y lower_leg_pos = readLinkState.value('link2_link', 'link') (knee_x, knee_y, knee_z) = [lower_leg_pos.link_state.pose.position.x, lower_leg_pos.link_state.pose.position.y, lower_leg_pos.link_state.pose.position.z] orientation_q = lower_leg_pos.link_state.pose.orientation orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] #matrix_k = quaternion_matrix(orientation_list) #matrix_k[:3,3] = [knee_x, knee_y, knee_z] (roll, pitch, yaw) = euler_from_quaternion (orientation_list) matrix_knee = compose_matrix(scale=None, shear=None, angles=[roll, pitch, yaw], translate=[knee_x, knee_y, knee_z], perspective=None) matrix_kf = compose_matrix(scale=None, shear=None, angles=[0, 0, 0], translate=[0, 0.08, 0], perspective=None) matrix_foot = np.dot(matrix_knee, matrix_kf) foot_z = matrix_foot[2][3]#-0.0239 if t > record_time: recorder_pos.record_entry(t, body_x, body_z, foot_z, q1, q3, phi1, phi3)
def se3_to_T(rvec, tvec): """Convert an se(3) OpenCV pose to a 4x4 transformation matrix. OpenCV poses are an Euler-Rodrigues rotation vector and a translation vector. """ R, _ = cv2.Rodrigues(rvec) euler_angles = transformations.euler_from_matrix(R) return transformations.compose_matrix(angles=euler_angles, translate=np.squeeze(tvec))
def ref_poseCB(self, goal_pose): # Takes target pose, returns ref se3 rospy.logdebug("ref_pose_cb called in velocity_control.py") # p = np.array([some_pose.position.x, some_pose.position.y, some_pose.position.z]) p = np.array([goal_pose.position.x, goal_pose.position.y + 0.04, goal_pose.position.z]) quat = np.array([goal_pose.orientation.x, goal_pose.orientation.y, goal_pose.orientation.z, goal_pose.orientation.w]) goal_tmp = tr.compose_matrix(angles=tr.euler_from_quaternion(quat, 'sxyz'), translate=p) # frame is spatial 'sxyz', return Euler angle from quaternion for specified axis sequence. with self.mutex: self.original_goal = goal_tmp
def tf_to_T(tf): """Convert a (tx, ty, tz, rx, ry, rz) tuple to a 4x4 transformation matrix.""" if len(tf) == 6: # Single transform return transformations.compose_matrix(angles=tf[3:], translate=tf[:3]) else: # Array of transforms assert tf.shape[1] == 6, "Transform array must be Nx6" return np.vstack([tf_to_T(row) for row in tf])
def evaluate(t_rc, camera_rb_poses, all_image_coords, all_calibration_markers, t_co, camera_mat, distortion_coeffs): # Get the optical frame pose in the mocap frame t_rc = transformations.compose_matrix(translate=t_rc[:3], angles=(t_rc[3:])) t_mos = calculate_t_mos(camera_rb_poses, t_rc, t_co) shape = (8, 5) square_size = 0.035 marker_z = 0.012 marker_positions = [ (-square_size, 0, -marker_z), (square_size * 8, 0, -marker_z), (square_size * 8, square_size * 5, -marker_z), (-square_size, square_size * 5, -marker_z), ] total_error = 0 for t_mo, image_coords, calibration_markers in zip( t_mos, all_image_coords, all_calibration_markers): checkerboard_coords = np.zeros((shape[0] * shape[1], 3), np.float32) checkerboard_coords[:, :2] = ( np.mgrid[0:shape[0], 0:shape[1]].T.reshape(-1, 2)) * square_size retval, rvec, tvec = cv2.solvePnP(checkerboard_coords, image_coords, camera_mat, distortion_coeffs) t_o_ch = calibration_common.se3_to_T(rvec, tvec) t_m_ch = np.matmul(t_mo, t_o_ch) total_distance = 0 for marker in marker_positions: marker = np.matmul(t_m_ch, np.append(marker, 1))[:3] match, distance = find_closest_marker_distance( marker, calibration_markers) print(match[2] - marker[2]) total_distance += distance total_distance /= 4 print("*" * 80) # To handle when the checkerboard was flipped during calibration checkerboard_coords = checkerboard_coords[::-1] retval, rvec, tvec = cv2.solvePnP(checkerboard_coords, image_coords, camera_mat, distortion_coeffs) t_o_ch = calibration_common.se3_to_T(rvec, tvec) t_m_ch = np.matmul(t_mo, t_o_ch) other_total_distance = 0 for marker in marker_positions: marker = np.matmul(t_m_ch, np.append(marker, 1))[:3] match, distance = find_closest_marker_distance( marker, calibration_markers) other_total_distance += distance print(match[2] - marker[2]) other_total_distance /= 4 if other_total_distance < total_distance: total_distance = other_total_distance total_error += total_distance return total_error / len(t_mos)
def _inverse_tuples(t): trans, rot = t euler = tr.euler_from_quaternion(rot) m = tr.compose_matrix(translate=trans, angles=euler) m_inv = tr.inverse_matrix(m) trans_inv = tr.translation_from_matrix(m_inv) rot_inv = tr.rotation_matrix(*tr.rotation_from_matrix(m_inv)) quat_inv = tr.quaternion_from_matrix(rot_inv) return (trans_inv, quat_inv)
def matrix_from_xyzquat_np_array(arg1, arg2=None): if arg2 is not None: translate = arg1 quaternion = arg2 else: translate = arg1[0:3] quaternion = arg1[3:7] return np.dot(compose_matrix(translate=translate),quaternion_matrix(quaternion))
def kinect_to_base(self,kinect_pose): try: time.sleep(1) (self.transl,self.quat)=self.listener.lookupTransform('base','kinect',rospy.Time(0)) self.rot = euler_from_quaternion(self.quat) self.tf_SE3 = compose_matrix(angles=self.rot,translate = self.transl) print self.tf_SE3 base_pos=numpy.dot(self.tf_SE3,kinect_pose) print base_pos # print self.cmd_pos except (tf.Exception): rospy.logerr("Could not transform from "\ "{0:s} to {1:s}".format(base,kinect)) pass return base_pos
def animate_2dsynced(data, shape_id, figfname): fig, ax = plt.subplots() fig.set_size_inches((7,7)) probe_radius = 0.004745 # probe1: 0.00626/2 probe2: 0.004745 sub = 1 # subsample rate tip_pose = data['tip_poses_2d'] object_pose = data['object_poses_2d'] force = data['force_2d'] patches = [] # add the object as polygon shape_db = ShapeDB() shape_polygon = shape_db.shape_db[shape_id]['shape'] # shape of the objects presented as polygon. shape_polygon_3d = np.hstack((np.array(shape_polygon), np.zeros((len(shape_polygon), 1)), np.ones((len(shape_polygon), 1)))) print 'object_pose', len(object_pose), 'tip_pose', len(tip_pose), 'force', len(force) plt.ion() for i in (range(0, len(tip_pose), sub)): plt.cla() T = tfm.compose_matrix(translate = object_pose[i][0:2] + [0], angles = (0,0,object_pose[i][2]) ) shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T) obj = mpatches.Polygon(shape_polygon_3d_world.T[:,0:2], closed=True, color='blue', alpha=0.05) ax.add_patch(obj) # add the probes as circle circle = mpatches.Circle(tip_pose[i][0:2], probe_radius, ec="none", color='red', alpha=0.5) ax.add_patch(circle) # add the force ax.arrow(tip_pose[i][0], tip_pose[i][1], force[i][0]/100, force[i][1]/100, head_width=0.005, head_length=0.01, fc='k', ec='k') #arrow = mpatches.Arrow(tip_pose[i][0], tip_pose[i][1], force[i][0], # force[i][1], head_width=0.05, head_length=0.1, fc='k', ec='k') #ax.add_patch(arrow) # render it plt.axis([-0.3, 0.3, -0.3, 0.3]) #plt.axis('equal') plt.draw() #time.sleep(0.1) plt.show()
def poseTomatrix(pose): """Converts a Pose message to a matrix 4. """ assert isinstance(pose, Pose) T = ((pose.position.x, pose.position.y, pose.position.z, ) ) R = transformations.euler_from_quaternion(( pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) ) return transformations.compose_matrix(translate=T, angles=R)
def goal_from_tag_pose(self, tag_pose): tf_mat = tft.compose_matrix(angles=self.grasp_transform[3:]) tag_q = (tag_pose.pose.orientation.x, tag_pose.pose.orientation.y, tag_pose.pose.orientation.z, tag_pose.pose.orientation.w) tag_mat = tft.quaternion_matrix(tag_q) new_mat = np.dot(tag_mat, tf_mat) dxyz = self.grasp_transform[:3] dxyz.append(1) d_xyz_base = np.matrix(dxyz).T d_xyz_tag = np.dot(tag_mat, d_xyz_base).A1[:3].tolist() new_q = tft.quaternion_from_matrix(new_mat) grasp_pose = PoseStamped() grasp_pose.header.stamp = rospy.Time.now() grasp_pose.header.frame_id = tag_pose.header.frame_id grasp_pose.pose.position.x = tag_pose.pose.position.x + d_xyz_tag[0] grasp_pose.pose.position.y = tag_pose.pose.position.y + d_xyz_tag[1] grasp_pose.pose.position.z = tag_pose.pose.position.z + d_xyz_tag[2] grasp_pose.pose.orientation = Quaternion(*new_q) self.test_pub_2.publish(grasp_pose) return grasp_pose
def tf_base_to_stylus (self): try: (self.transl,self.quat) = self.tfListener.lookupTransform('base','stylus',rospy.Time(0)) #lookupTransrom is a method which returns the transfrom between two coordinate frames. #lookupTransfrom returns a translation and a quaternion self.rot= euler_from_quaternion(self.quat) #Get euler angles from the quaternion self.tf_SE3 = compose_matrix(angles=self.rot,translate=self.transl) #Store the transformation in a format compatible with gemoetr_msgs/Twist self.transf = Twist(Vector3(self.transl[0],self.transl[1],self.transl[2]),(Vector3(self.rot[0],self.rot[1],self.rot[2]))) #Publish the transformation self.Tsb.publish(self.transf) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): self.tf_SE3 = None pass #If exceptions occur, skip trying to lookup the transform. #It is encouraged to publish a logging message to rosout with rospy. #i.e: #rospy.logerr("Could not transform from %s to %s,"base","stylus") return
def params_to_matrix(self, params): # uses euler angles.... assumes we aren't near singularities translate = params[0:3] angles = params[3:6] return transformations.compose_matrix(translate=translate, angles=angles)
def matrix_from_xyzrpy(translate, rpy): return np.dot(tfm.compose_matrix(translate=translate) , tfm.euler_matrix(*rpy)).tolist()
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception) as e: rospy.logwarn("Could not find transformation: " + str(e)) continue # rospy.loginfo("Got the matrix:\n%s", transformation_matrix) translation = transformation_matrix[:3, 3] rotation = transformation_matrix[:3, :3] permutation_matrix = numpy.array([[0, 0., -1], [-1, 0, 0], [0, 1, 0]]) new_rotation = permutation_matrix.dot(rotation) R = numpy.eye(4) R[:3, :3] = new_rotation T1 = transformations.compose_matrix(translate= -translation) T2 = transformations.compose_matrix(translate=translation) # full_transformation_matrix = T1.dot(R).dot(T2) full_transformation_matrix = T2.dot(R).dot(T1) # rospy.loginfo("Full transformation matrix:\n%s", full_transformation_matrix) resulting_matrix = full_transformation_matrix.dot(transformation_matrix) # rospy.loginfo("The result is:\n%s", resulting_matrix) full_translation = transformations.translation_from_matrix(resulting_matrix) full_rotation_q = transformations.quaternion_from_matrix(resulting_matrix) br.sendTransform(full_translation, full_rotation_q, hdr.stamp, "flipped_frame", parent_frame)
def plan(self): # plan() return an object includes attributes # q_traj, # snopt_info_iktraj, # infeasible_constraint_iktraj # snopt_info_ik # 1. Get a handle for the service # todo: makes warning if ikTrajServer does not exist ikTrajServer_pub = rospy.Publisher('/ikTrajServer', std_msgs.msg.String, queue_size=100) argin = {} # 2. prepare input arguments if self.q0 is not None: argin['q0'] = self.q0 else: # get robot joints from published topic # sensor_msgs/JointState while True: APCrobotjoints = ROS_Wait_For_Msg(self.joint_topic, sensor_msgs.msg.JointState).getmsg() q0 = APCrobotjoints.position if len(q0) < 6: continue else: argin['q0'] = q0 break # 2.1 transform tip pose to hand pose if self.target_tip_pos is not None: tip_hand_tfm_mat = np.dot( tfm.compose_matrix(translate=self.tip_hand_transform[0:3]), tfm.compose_matrix(angles=self.tip_hand_transform[3:6]) ) tip_world_rot_mat = tfm.quaternion_matrix(self.target_tip_ori) tip_world_tfm_mat = np.dot(tfm.compose_matrix(translate=self.target_tip_pos) , tip_world_rot_mat) hand_tip_tfm_mat = np.linalg.inv(tip_hand_tfm_mat) hand_world_tfm_mat = np.dot(tip_world_tfm_mat, hand_tip_tfm_mat) target_hand_pos = tfm.translation_from_matrix(hand_world_tfm_mat) argin['target_hand_pos'] = target_hand_pos.tolist() if self.target_tip_ori is not None: tip_hand_tfm_mat = tfm.compose_matrix(angles=self.tip_hand_transform[3:6]) tip_world_rot_mat = tfm.quaternion_matrix(self.target_tip_ori) hand_tip_tfm_mat = np.linalg.inv(tip_hand_tfm_mat) hand_world_tfm_mat = np.dot(tip_world_tfm_mat, hand_tip_tfm_mat) target_hand_ori = tfm.quaternion_from_matrix(hand_world_tfm_mat) argin['target_hand_ori'] = roshelper.ros2matlabQuat(target_hand_ori.tolist()) # tolist: so that its serializable # 2.2 prepare other options argin['straightness'] = self.straightness argin['target_link'] = self.target_link argin['pos_tol'] = self.pos_tol argin['ori_tol'] = self.ori_tol if self.inframebb is not None: argin['inframebb'] = self.inframebb if self.target_joint_bb is not None: argin['target_joint_bb'] = self.target_joint_bb argin['N'] = self.N argin['tip_hand_transform'] = self.tip_hand_transform if self.ik_only: argin['ik_only'] = 1 else: argin['ik_only'] = 0 argin_json = json.dumps(argin) # convert to json format and for i in range(10): # 3. call it #print 'calling matlab drake...' argin_json_ = argin_json + '\n' try: tn = telnetlib.Telnet(self.ikServerAddress[0], self.ikServerAddress[1]) time.sleep(0.05) tn.write(argin_json_) except socket.error as inst: print '[Drake] Matlab server not up now.' if i==9: return None continue time.sleep(0.05) # 4. parse output arguments try: ret_json = tn.read_all() ret = json.loads(ret_json) # convert from json to struct break except: print '[Drake] read_all() failed, try again.' if i==9: return None continue return Plan(ret)
def matrix_from_xyzquat(translate, quaternion): return np.dot(tfm.compose_matrix(translate=translate) , tfm.quaternion_matrix(quaternion)).tolist()